mirror of
https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git
synced 2023-05-28 12:51:38 +08:00
fast_lio2 released!
This commit is contained in:
xw
123
include/IKFoM_toolkit/mtk/src/SubManifold.hpp
Executable file
123
include/IKFoM_toolkit/mtk/src/SubManifold.hpp
Executable file
@@ -0,0 +1,123 @@
|
||||
// This is an advanced implementation of the algorithm described in the
|
||||
// following paper:
|
||||
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
|
||||
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/src/SubManifold.hpp
|
||||
* @brief Defines the SubManifold class
|
||||
*/
|
||||
|
||||
|
||||
#ifndef SUBMANIFOLD_HPP_
|
||||
#define SUBMANIFOLD_HPP_
|
||||
|
||||
|
||||
#include "vectview.hpp"
|
||||
|
||||
|
||||
namespace MTK {
|
||||
|
||||
/**
|
||||
* @ingroup SubManifolds
|
||||
* Helper class for compound manifolds.
|
||||
* This class wraps a manifold T and provides an enum IDX refering to the
|
||||
* index of the SubManifold within the compound manifold.
|
||||
*
|
||||
* Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds".
|
||||
*
|
||||
* @tparam T The manifold type of the sub-type
|
||||
* @tparam idx The index of the sub-type within the compound manifold
|
||||
*/
|
||||
template<class T, int idx, int dim>
|
||||
struct SubManifold : public T
|
||||
{
|
||||
enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ };
|
||||
//! manifold type
|
||||
typedef T type;
|
||||
|
||||
//! Construct from derived type
|
||||
template<class X>
|
||||
explicit
|
||||
SubManifold(const X& t) : T(t) {};
|
||||
|
||||
//! Construct from internal type
|
||||
//explicit
|
||||
SubManifold(const T& t) : T(t) {};
|
||||
|
||||
//! inherit assignment operator
|
||||
using T::operator=;
|
||||
|
||||
};
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
|
||||
#endif /* SUBMANIFOLD_HPP_ */
|
||||
294
include/IKFoM_toolkit/mtk/src/mtkmath.hpp
Executable file
294
include/IKFoM_toolkit/mtk/src/mtkmath.hpp
Executable file
@@ -0,0 +1,294 @@
|
||||
// This is an advanced implementation of the algorithm described in the
|
||||
// following paper:
|
||||
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
|
||||
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/src/mtkmath.hpp
|
||||
* @brief several math utility functions.
|
||||
*/
|
||||
|
||||
#ifndef MTKMATH_H_
|
||||
#define MTKMATH_H_
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <boost/math/tools/precision.hpp>
|
||||
|
||||
#include "../types/vect.hpp"
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.1415926535897932384626433832795
|
||||
#endif
|
||||
|
||||
|
||||
namespace MTK {
|
||||
|
||||
namespace internal {
|
||||
|
||||
template<class Manifold>
|
||||
struct traits {
|
||||
typedef typename Manifold::scalar scalar;
|
||||
enum {DOF = Manifold::DOF};
|
||||
typedef vect<DOF, scalar> vectorized_type;
|
||||
typedef Eigen::Matrix<scalar, DOF, DOF> matrix_type;
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<float> : traits<Scalar<float> > {};
|
||||
template<>
|
||||
struct traits<double> : traits<Scalar<double> > {};
|
||||
|
||||
} // namespace internal
|
||||
|
||||
/**
|
||||
* \defgroup MTKMath Mathematical helper functions
|
||||
*/
|
||||
//@{
|
||||
|
||||
//! constant @f$ \pi @f$
|
||||
const double pi = M_PI;
|
||||
|
||||
template<class scalar> inline scalar tolerance();
|
||||
|
||||
template<> inline float tolerance<float >() { return 1e-5f; }
|
||||
template<> inline double tolerance<double>() { return 1e-11; }
|
||||
|
||||
|
||||
/**
|
||||
* normalize @a x to @f$[-bound, bound] @f$.
|
||||
*
|
||||
* result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$.
|
||||
*/
|
||||
template<class scalar>
|
||||
inline scalar normalize(scalar x, scalar bound){ //not used
|
||||
if(std::fabs(x) <= bound) return x;
|
||||
int r = (int)(x *(scalar(1.0)/ bound));
|
||||
return x - ((r + (r>>31) + 1) & ~1)*bound;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate cosine and sinc of sqrt(x2).
|
||||
* @param x2 the squared angle must be non-negative
|
||||
* @return a pair containing cos and sinc of sqrt(x2)
|
||||
*/
|
||||
template<class scalar>
|
||||
std::pair<scalar, scalar> cos_sinc_sqrt(const scalar &x2){
|
||||
using std::sqrt;
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
static scalar const taylor_0_bound = boost::math::tools::epsilon<scalar>();
|
||||
static scalar const taylor_2_bound = sqrt(taylor_0_bound);
|
||||
static scalar const taylor_n_bound = sqrt(taylor_2_bound);
|
||||
|
||||
assert(x2>=0 && "argument must be non-negative");
|
||||
|
||||
// FIXME check if bigger bounds are possible
|
||||
if(x2>=taylor_n_bound) {
|
||||
// slow fall-back solution
|
||||
scalar x = sqrt(x2);
|
||||
return std::make_pair(cos(x), sin(x)/x); // x is greater than 0.
|
||||
}
|
||||
|
||||
// FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd)
|
||||
// TODO Find optimal coefficients using Remez algorithm
|
||||
static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.};
|
||||
scalar cosi = 1., sinc=1;
|
||||
scalar term = -1/2. * x2;
|
||||
for(int i=0; i<3; ++i) {
|
||||
cosi += term;
|
||||
term *= inv[2*i];
|
||||
sinc += term;
|
||||
term *= -inv[2*i+1] * x2;
|
||||
}
|
||||
|
||||
return std::make_pair(cosi, sinc);
|
||||
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> hat(const Base& v) {
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> res;
|
||||
res << 0, -v[2], v[1],
|
||||
v[2], 0, -v[0],
|
||||
-v[1], v[0], 0;
|
||||
return res;
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> A_inv_trans(const Base& v){
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> res;
|
||||
if(v.norm() > MTK::tolerance<typename Base::scalar>())
|
||||
{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + 0.5 * hat<Base>(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> A_inv(const Base& v){
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> res;
|
||||
if(v.norm() > MTK::tolerance<typename Base::scalar>())
|
||||
{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() - 0.5 * hat<Base>(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
template<typename scalar>
|
||||
Eigen::Matrix<scalar, 2, 3> S2_w_expw_( Eigen::Matrix<scalar, 2, 1> v, scalar length)
|
||||
{
|
||||
Eigen::Matrix<scalar, 2, 3> res;
|
||||
scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]);
|
||||
if(norm < MTK::tolerance<scalar>()){
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
res(0, 1) = 1;
|
||||
res(1, 2) = 1;
|
||||
res /= length;
|
||||
}
|
||||
else{
|
||||
res << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0,
|
||||
-v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm);
|
||||
res /= length;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> A_matrix(const Base & v){
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> res;
|
||||
double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
|
||||
double norm = std::sqrt(squaredNorm);
|
||||
if(norm < MTK::tolerance<typename Base::scalar>()){
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
|
||||
}
|
||||
else{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
template<class scalar, int n>
|
||||
scalar exp(vectview<scalar, n> result, vectview<const scalar, n> vec, const scalar& scale = 1) {
|
||||
scalar norm2 = vec.squaredNorm();
|
||||
std::pair<scalar, scalar> cos_sinc = cos_sinc_sqrt(scale*scale * norm2);
|
||||
scalar mult = cos_sinc.second * scale;
|
||||
result = mult * vec;
|
||||
return cos_sinc.first;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Inverse function to @c exp.
|
||||
*
|
||||
* @param result @c vectview to the result
|
||||
* @param w scalar part of input
|
||||
* @param vec vector part of input
|
||||
* @param scale scale result by this value
|
||||
* @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result
|
||||
*/
|
||||
template<class scalar, int n>
|
||||
void log(vectview<scalar, n> result,
|
||||
const scalar &w, const vectview<const scalar, n> vec,
|
||||
const scalar &scale, bool plus_minus_periodicity)
|
||||
{
|
||||
// FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division
|
||||
scalar nv = vec.norm();
|
||||
if(nv < tolerance<scalar>()) {
|
||||
if(!plus_minus_periodicity && w < 0) {
|
||||
// find the maximal entry:
|
||||
int i;
|
||||
nv = vec.cwiseAbs().maxCoeff(&i);
|
||||
result = scale * std::atan2(nv, w) * vect<n, scalar>::Unit(i);
|
||||
return;
|
||||
}
|
||||
nv = tolerance<scalar>();
|
||||
}
|
||||
scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) );
|
||||
|
||||
result = s * vec;
|
||||
}
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
|
||||
#endif /* MTKMATH_H_ */
|
||||
168
include/IKFoM_toolkit/mtk/src/vectview.hpp
Executable file
168
include/IKFoM_toolkit/mtk/src/vectview.hpp
Executable file
@@ -0,0 +1,168 @@
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/src/vectview.hpp
|
||||
* @brief Wrapper class around a pointer used as interface for plain vectors.
|
||||
*/
|
||||
|
||||
#ifndef VECTVIEW_HPP_
|
||||
#define VECTVIEW_HPP_
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
namespace MTK {
|
||||
|
||||
/**
|
||||
* A view to a vector.
|
||||
* Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions.
|
||||
* The dimension of the vector is given as template parameter and type-checked when used in expressions.
|
||||
* Data has to be modifiable.
|
||||
*
|
||||
* @tparam scalar Scalar type of the vector.
|
||||
* @tparam dim Dimension of the vector.
|
||||
*
|
||||
* @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct
|
||||
*/
|
||||
namespace internal {
|
||||
template<class Base, class T1, class T2>
|
||||
struct CovBlock {
|
||||
typedef typename Eigen::Block<Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>, T1::DOF, T2::DOF> Type;
|
||||
typedef typename Eigen::Block<const Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>, T1::DOF, T2::DOF> ConstType;
|
||||
};
|
||||
|
||||
template<class Base, class T1, class T2>
|
||||
struct CovBlock_ {
|
||||
typedef typename Eigen::Block<Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>, T1::DIM, T2::DIM> Type;
|
||||
typedef typename Eigen::Block<const Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>, T1::DIM, T2::DIM> ConstType;
|
||||
};
|
||||
|
||||
template<typename Base1, typename Base2, typename T1, typename T2>
|
||||
struct CrossCovBlock {
|
||||
typedef typename Eigen::Block<Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF>, T1::DOF, T2::DOF> Type;
|
||||
typedef typename Eigen::Block<const Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF>, T1::DOF, T2::DOF> ConstType;
|
||||
};
|
||||
|
||||
template<typename Base1, typename Base2, typename T1, typename T2>
|
||||
struct CrossCovBlock_ {
|
||||
typedef typename Eigen::Block<Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM>, T1::DIM, T2::DIM> Type;
|
||||
typedef typename Eigen::Block<const Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM>, T1::DIM, T2::DIM> ConstType;
|
||||
};
|
||||
|
||||
template<class scalar, int dim>
|
||||
struct VectviewBase {
|
||||
typedef Eigen::Matrix<scalar, dim, 1> matrix_type;
|
||||
typedef typename matrix_type::MapType Type;
|
||||
typedef typename matrix_type::ConstMapType ConstType;
|
||||
};
|
||||
|
||||
template<class T>
|
||||
struct UnalignedType {
|
||||
typedef T type;
|
||||
};
|
||||
}
|
||||
|
||||
template<class scalar, int dim>
|
||||
class vectview : public internal::VectviewBase<scalar, dim>::Type {
|
||||
typedef internal::VectviewBase<scalar, dim> VectviewBase;
|
||||
public:
|
||||
//! plain matrix type
|
||||
typedef typename VectviewBase::matrix_type matrix_type;
|
||||
//! base type
|
||||
typedef typename VectviewBase::Type base;
|
||||
//! construct from pointer
|
||||
explicit
|
||||
vectview(scalar* data, int dim_=dim) : base(data, dim_) {}
|
||||
//! construct from plain matrix
|
||||
vectview(matrix_type& m) : base(m.data(), m.size()) {}
|
||||
//! construct from another @c vectview
|
||||
vectview(const vectview &v) : base(v) {}
|
||||
//! construct from Eigen::Block:
|
||||
template<class Base>
|
||||
vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef(0), block.size()) {}
|
||||
template<class Base, bool PacketAccess>
|
||||
vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&block.coeffRef(0), block.size()) {}
|
||||
|
||||
//! inherit assignment operator
|
||||
using base::operator=;
|
||||
//! data pointer
|
||||
scalar* data() {return const_cast<scalar*>(base::data());}
|
||||
};
|
||||
|
||||
/**
|
||||
* @c const version of @c vectview.
|
||||
* Compared to @c Eigen::Map this implementation is const correct, i.e.,
|
||||
* data will not be modifiable using this view.
|
||||
*
|
||||
* @tparam scalar Scalar type of the vector.
|
||||
* @tparam dim Dimension of the vector.
|
||||
*
|
||||
* @sa vectview
|
||||
*/
|
||||
template<class scalar, int dim>
|
||||
class vectview<const scalar, dim> : public internal::VectviewBase<scalar, dim>::ConstType {
|
||||
typedef internal::VectviewBase<scalar, dim> VectviewBase;
|
||||
public:
|
||||
//! plain matrix type
|
||||
typedef typename VectviewBase::matrix_type matrix_type;
|
||||
//! base type
|
||||
typedef typename VectviewBase::ConstType base;
|
||||
//! construct from const pointer
|
||||
explicit
|
||||
vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {}
|
||||
//! construct from column vector
|
||||
template<int options>
|
||||
vectview(const Eigen::Matrix<scalar, dim, 1, options>& m) : base(m.data()) {}
|
||||
//! construct from row vector
|
||||
template<int options, int phony>
|
||||
vectview(const Eigen::Matrix<scalar, 1, dim, options, phony>& m) : base(m.data()) {}
|
||||
//! construct from another @c vectview
|
||||
vectview(vectview<scalar, dim> x) : base(x.data()) {}
|
||||
//! construct from base
|
||||
vectview(const base &x) : base(x) {}
|
||||
/**
|
||||
* Construct from Block
|
||||
* @todo adapt this, when Block gets const-correct
|
||||
*/
|
||||
template<class Base>
|
||||
vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef(0)) {}
|
||||
template<class Base, bool PacketAccess>
|
||||
vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&block.coeffRef(0)) {}
|
||||
|
||||
};
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
#endif /* VECTVIEW_HPP_ */
|
||||
Reference in New Issue
Block a user