Eigen.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.

 
#pragma once
 
#include <util/Logger.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
 
#include <nlohmann/json.hpp>
using json = nlohmann::json; 
 
#include <fmt/ostream.h>
 
#include "Assert.h"
 
namespace Eigen
{
using Array5d = Array<double, 5, 1>;   
using Array6d = Array<double, 6, 1>;   
using Vector5d = Matrix<double, 5, 1>; 
using Vector6d = Matrix<double, 6, 1>; 
using Vector7d = Matrix<double, 7, 1>; 
using Vector8d = Matrix<double, 8, 1>; 
using Vector9d = Matrix<double, 9, 1>; 
using Matrix5d = Matrix<double, 5, 5>; 
using Matrix6d = Matrix<double, 6, 6>; 
using Matrix7d = Matrix<double, 7, 7>; 
using Matrix8d = Matrix<double, 8, 8>; 
using Matrix9d = Matrix<double, 9, 9>; 
 
using Array3ld = Array<long double, 3, 1>; 
 
using Vector3ld = Matrix<long double, 3, 1>; 
using Vector4ld = Matrix<long double, 4, 1>; 
 
using Matrix3ld = Matrix<long double, 3, 3>; 
using Matrix4ld = Matrix<long double, 4, 4>; 
 
using Quaternionld = Quaternion<long double>; 
 
using AngleAxisld = AngleAxis<long double>; 

template<typename _Scalar, int _Rows, int _Cols>
void to_json(json& j, const Matrix<_Scalar, _Rows, _Cols>& matrix)
{
    for (int r = 0; r < matrix.rows(); r++)
    {
        for (int c = 0; c < matrix.cols(); c++)
        {
            if (std::isnan(matrix(r, c))) { j[std::to_string(r)][std::to_string(c)] = "NaN"; }
            else { j[std::to_string(r)][std::to_string(c)] = matrix(r, c); }
        }
    }
}

template<typename _Scalar, int _Rows, int _Cols>
void from_json(const json& j, Matrix<_Scalar, _Rows, _Cols>& matrix)
{
    if constexpr (_Rows == -1 || _Cols == -1)
    {
        int rows = -1;
        int cols = -1;
        for (int r = 0; j.contains(std::to_string(r)); r++)
        {
            rows = std::max(r, rows);
            for (int c = 0; j.at(std::to_string(r)).contains(std::to_string(c)); c++)
            {
                cols = std::max(c, cols);
            }
        }
        matrix = Eigen::Matrix<_Scalar, _Rows, _Cols>::Zero(rows + 1, cols + 1);
    }
 
    for (int r = 0; j.contains(std::to_string(r)); r++) // NOLINT(readability-misleading-indentation)
    {
        for (int c = 0; j.at(std::to_string(r)).contains(std::to_string(c)); c++)
        {
            if (j.at(std::to_string(r)).at(std::to_string(c)).is_string())
            {
                auto str = j.at(std::to_string(r)).at(std::to_string(c)).get<std::string>();
                if (str == "NaN") { matrix(r, c) = std::nan(""); }
                else { LOG_WARN("Reading matrix value failed at position ({}, {}). Value is an unknown string '{}'.", r, c, str); }
            }
            else if (j.at(std::to_string(r)).at(std::to_string(c)).is_number())
            {
                j.at(std::to_string(r)).at(std::to_string(c)).get_to(matrix(r, c));
            }
            else
            {
                LOG_WARN("Reading matrix value failed at position ({}, {}). Value has the type '{}' which cannot be converted into a floating point number.", r, c, j.at(std::to_string(r)).at(std::to_string(c)).type_name());
            }
        }
    }
}

template<typename _Scalar>
void to_json(json& j, const Quaternion<_Scalar>& quat)
{
    to_json(j, quat.coeffs());
}

template<typename _Scalar>
void from_json(const json& j, Quaternion<_Scalar>& quat)
{
    from_json(j, quat.coeffs());
}
 
} // namespace Eigen
 
namespace NAV
{

template<typename Derived>
void removeRows(Eigen::DenseBase<Derived>& matrix, size_t index, size_t length)
{
    INS_ASSERT_USER_ERROR(static_cast<size_t>(matrix.rows()) >= index + length, "Tried to remove rows which do not exist");
 
    std::vector<int> indicesToKeep;
    indicesToKeep.reserve(static_cast<size_t>(matrix.rows()) - length);
    for (int i = 0; i < static_cast<int>(index); i++) { indicesToKeep.push_back(i); }
    for (int i = static_cast<int>(index + length); i < matrix.rows(); i++) { indicesToKeep.push_back(i); }
 
    matrix = matrix(indicesToKeep, Eigen::all).eval();
}

template<typename Derived>
void removeRows(Eigen::DenseBase<Derived>& matrix, const std::vector<int>& rowIndices)
{
    std::vector<int> rowIndicesToKeep;
    rowIndicesToKeep.reserve(static_cast<size_t>(matrix.rows()) - rowIndices.size());
    for (int i = 0; i < matrix.rows(); i++)
    {
        if (std::ranges::find(rowIndices, i) == rowIndices.end())
        {
            rowIndicesToKeep.push_back(i);
        }
    }
 
    matrix = matrix(rowIndicesToKeep, Eigen::all).eval();
}

template<typename Derived>
void removeCols(Eigen::DenseBase<Derived>& matrix, size_t index, size_t length)
{
    INS_ASSERT_USER_ERROR(static_cast<size_t>(matrix.cols()) >= index + length, "Tried to remove cols which do not exist");
 
    std::vector<int> indicesToKeep;
    indicesToKeep.reserve(static_cast<size_t>(matrix.cols()) - length);
    for (int i = 0; i < static_cast<int>(index); i++) { indicesToKeep.push_back(i); }
    for (int i = static_cast<int>(index + length); i < matrix.cols(); i++) { indicesToKeep.push_back(i); }
 
    matrix = matrix(Eigen::all, indicesToKeep).eval();
}

template<typename Derived>
void removeCols(Eigen::DenseBase<Derived>& matrix, const std::vector<int>& colIndices)
{
    std::vector<int> colIndicesToKeep;
    colIndicesToKeep.reserve(static_cast<size_t>(matrix.cols()) - colIndices.size());
    for (int i = 0; i < matrix.cols(); i++)
    {
        if (std::ranges::find(colIndices, i) == colIndices.end())
        {
            colIndicesToKeep.push_back(i);
        }
    }
 
    matrix = matrix(Eigen::all, colIndicesToKeep).eval();
}

template<typename Derived>
void removeRowsAndCols(Eigen::DenseBase<Derived>& matrix, size_t row, size_t rows, size_t col, size_t cols)
{
    INS_ASSERT_USER_ERROR(static_cast<size_t>(matrix.rows()) >= row + rows, "Tried to remove rows which do not exist");
    INS_ASSERT_USER_ERROR(static_cast<size_t>(matrix.cols()) >= col + cols, "Tried to remove cols which do not exist");
 
    std::vector<int> rowsToKeep;
    rowsToKeep.reserve(static_cast<size_t>(matrix.rows()) - rows);
    for (int i = 0; i < static_cast<int>(row); i++) { rowsToKeep.push_back(i); }
    for (int i = static_cast<int>(row + rows); i < matrix.rows(); i++) { rowsToKeep.push_back(i); }
 
    std::vector<int> colsToKeep;
    colsToKeep.reserve(static_cast<size_t>(matrix.cols()) - cols);
    for (int i = 0; i < static_cast<int>(col); i++) { colsToKeep.push_back(i); }
    for (int i = static_cast<int>(col + cols); i < matrix.cols(); i++) { colsToKeep.push_back(i); }
 
    matrix = matrix(rowsToKeep, colsToKeep).eval();
}

template<typename Derived>
void removeRowsAndCols(Eigen::DenseBase<Derived>& matrix, const std::vector<int>& rowIndices, const std::vector<int>& colIndices)
{
    std::vector<int> rowIndicesToKeep;
    rowIndicesToKeep.reserve(static_cast<size_t>(matrix.rows()) - rowIndices.size());
    for (int i = 0; i < matrix.rows(); i++)
    {
        if (std::ranges::find(rowIndices, i) == rowIndices.end())
        {
            rowIndicesToKeep.push_back(i);
        }
    }
 
    std::vector<int> colIndicesToKeep;
    colIndicesToKeep.reserve(static_cast<size_t>(matrix.cols()) - colIndices.size());
    for (int i = 0; i < matrix.cols(); i++)
    {
        if (std::ranges::find(colIndices, i) == colIndices.end())
        {
            colIndicesToKeep.push_back(i);
        }
    }
 
    matrix = matrix(rowIndicesToKeep, colIndicesToKeep).eval();
}
 
} // namespace NAV
 
#ifndef DOXYGEN_IGNORE
 
// clang-format off
 
template<typename T>
    requires std::is_base_of_v<Eigen::DenseBase<T>, T>
struct fmt::formatter<T> : ostream_formatter
{};
 
template<typename T>
struct fmt::formatter<Eigen::MatrixBase<T>> : ostream_formatter
{};
 
 
template<typename T>
requires std::is_base_of_v<Eigen::QuaternionBase<T>, T>
struct fmt::formatter<T> : ostream_formatter
{};
 
template<typename T>
struct fmt::formatter<Eigen::QuaternionBase<T>> : ostream_formatter
{};
 
// clang-format on
 
#endif