Eigen.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#ifndef CLOUDVIEWER_EIGEN_H
#define CLOUDVIEWER_EIGEN_H
 
#include "CVCoreLib.h"
 
#ifdef _MSVC_LANG
#define CPP_VERSION _MSVC_LANG
#else
#define CPP_VERSION __cplusplus
#endif
 
// EIGEN
// Ensure EIGEN_HAS_CXX17_OVERALIGN is set before including Eigen headers
// This allows C++17 std::allocator's automatic alignment handling even when
// EIGEN_MAX_ALIGN_BYTES=0 (for PCL compatibility)
#ifndef EIGEN_HAS_CXX17_OVERALIGN
#ifdef CPP_VERSION
#if CPP_VERSION >= 201703L
#define EIGEN_HAS_CXX17_OVERALIGN 1
#endif
#endif
#endif
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
 
// SYSTEM
#include <memory>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>
 
#if !EIGEN_VERSION_AT_LEAST(3, 4, 0) || CPP_VERSION < 201703L
 
#include <initializer_list>
#ifndef EIGEN_ALIGNED_ALLOCATOR
#define EIGEN_ALIGNED_ALLOCATOR Eigen::aligned_allocator
#endif
 
// Equivalent to EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION but with support for
// initializer lists, which is a C++11 feature and not supported by the Eigen.
// The initializer list extension is inspired by Theia and StackOverflow code.
#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(...)                   \
    namespace std {                                                          \
    template <>                                                              \
    class vector<__VA_ARGS__, std::allocator<__VA_ARGS__>>                   \
        : public vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__>> { \
        typedef vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__>>    \
                vector_base;                                                 \
                                                                             \
    public:                                                                  \
        typedef __VA_ARGS__ value_type;                                      \
        typedef vector_base::allocator_type allocator_type;                  \
        typedef vector_base::size_type size_type;                            \
        typedef vector_base::iterator iterator;                              \
        explicit vector(const allocator_type& a = allocator_type())          \
            : vector_base(a) {}                                              \
        template <typename InputIterator>                                    \
        vector(InputIterator first,                                          \
               InputIterator last,                                           \
               const allocator_type& a = allocator_type())                   \
            : vector_base(first, last, a) {}                                 \
        vector(const vector& c) : vector_base(c) {}                          \
        explicit vector(size_type num, const value_type& val = value_type()) \
            : vector_base(num, val) {}                                       \
        vector(iterator start, iterator end) : vector_base(start, end) {}    \
        vector& operator=(const vector& x) {                                 \
            vector_base::operator=(x);                                       \
            return *this;                                                    \
        }                                                                    \
        vector(initializer_list<__VA_ARGS__> list)                           \
            : vector_base(list.begin(), list.end()) {}                       \
    };                                                                       \
    }  // namespace std
 
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector2d)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector4d)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector4f)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix2d)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix2f)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix4d)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix4f)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Affine3d)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Affine3f)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Quaterniond)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Quaternionf)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix<float, 3, 4>)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix<double, 3, 4>)
 
#endif
 
#undef CPP_VERSION
 
namespace Eigen {
 
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<uint8_t, 3, 1> Vector3uint8;
 
typedef Eigen::Matrix<double, 6, 6, Eigen::DontAlign> Matrix6d_u;
typedef Eigen::Matrix<double, 4, 4, Eigen::DontAlign> Matrix4d_u;
typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> Vector3d_u;
typedef Eigen::Matrix<float, 3, 1, Eigen::DontAlign> Vector3f_u;
typedef Eigen::Matrix<double, 4, 1, Eigen::DontAlign> Vector4d_u;
typedef Eigen::Matrix<float, 4, 1, Eigen::DontAlign> Vector4f_u;
 
}  // namespace Eigen
 
namespace cloudViewer {
 
namespace utility {
 
using Matrix4d_allocator = Eigen::aligned_allocator<Eigen::Matrix4d>;
using Matrix6d_allocator = Eigen::aligned_allocator<Eigen::Matrix6d>;
using Vector2d_allocator = Eigen::aligned_allocator<Eigen::Vector2d>;
using Vector3uint8_allocator = Eigen::aligned_allocator<Eigen::Vector3uint8>;
using Vector4i_allocator = Eigen::aligned_allocator<Eigen::Vector4i>;
using Vector4d_allocator = Eigen::aligned_allocator<Eigen::Vector4d>;
using Vector6d_allocator = Eigen::aligned_allocator<Eigen::Vector6d>;
 
Eigen::Matrix3d CV_CORE_LIB_API SkewMatrix(const Eigen::Vector3d& vec);
 
Eigen::Matrix4d CV_CORE_LIB_API
TransformVector6dToMatrix4d(const Eigen::Vector6d& input);
 
Eigen::Vector6d CV_CORE_LIB_API
TransformMatrix4dToVector6d(const Eigen::Matrix4d& input);
 
std::tuple<bool, Eigen::VectorXd> CV_CORE_LIB_API
SolveLinearSystemPSD(const Eigen::MatrixXd& A,
                     const Eigen::VectorXd& b,
                     bool prefer_sparse = false,
                     bool check_symmetric = false,
                     bool check_det = false,
                     bool check_psd = false);
 
std::tuple<bool, Eigen::Matrix4d> CV_CORE_LIB_API
SolveJacobianSystemAndObtainExtrinsicMatrix(const Eigen::Matrix6d& JTJ,
                                            const Eigen::Vector6d& JTr);
 
std::tuple<bool, std::vector<Eigen::Matrix4d, Matrix4d_allocator>>
        CV_CORE_LIB_API SolveJacobianSystemAndObtainExtrinsicMatrixArray(
                const Eigen::MatrixXd& JTJ, const Eigen::VectorXd& JTr);
 
template <typename MatType, typename VecType>
std::tuple<MatType, VecType, double> CV_CORE_LIB_API
ComputeJTJandJTr(std::function<void(int, VecType&, double&, double&)> f,
                 int iteration_num,
                 bool verbose = true);
 
template <typename MatType, typename VecType>
std::tuple<MatType, VecType, double> CV_CORE_LIB_API ComputeJTJandJTr(
        std::function<
                void(int,
                     std::vector<VecType, Eigen::aligned_allocator<VecType>>&,
                     std::vector<double>&,
                     std::vector<double>&)> f,
        int iteration_num,
        bool verbose = true);
 
Eigen::Matrix3d CV_CORE_LIB_API RotationMatrixX(double radians);
Eigen::Matrix3d CV_CORE_LIB_API RotationMatrixY(double radians);
Eigen::Matrix3d CV_CORE_LIB_API RotationMatrixZ(double radians);
 
Eigen::Vector3uint8 CV_CORE_LIB_API ColorToUint8(const Eigen::Vector3d& color);
Eigen::Vector3d CV_CORE_LIB_API ColorToDouble(uint8_t r, uint8_t g, uint8_t b);
Eigen::Vector3d CV_CORE_LIB_API ColorToDouble(const Eigen::Vector3uint8& rgb);
 
template <typename IdxType>
Eigen::Matrix3d CV_CORE_LIB_API
ComputeCovariance(const std::vector<Eigen::Vector3d>& points,
                  const std::vector<IdxType>& indices);
 
template <typename IdxType>
std::tuple<Eigen::Vector3d, Eigen::Matrix3d> CV_CORE_LIB_API
ComputeMeanAndCovariance(const std::vector<Eigen::Vector3d>& points,
                         const std::vector<IdxType>& indices);
 
template <typename RealType, typename IdxType>
std::tuple<Eigen::Vector3d, Eigen::Matrix3d> CV_CORE_LIB_API
ComputeMeanAndCovariance(const RealType* const points,
                         const std::vector<IdxType>& indices);
 
}  // namespace utility
}  // namespace cloudViewer
 
#endif  // CLOUDVIEWER_EIGEN_H