Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Support formatting more Eigen types (#4391)

Browse Source 
Added an Eigen::SparseMatrix formatter.

Also modified the Eigen::Matrix formatter to support Eigen::MatrixXd.
Eigen::MatrixXd sets both dimension template arguments to -1, so they
can't be used for iteration. rows() and cols() are now used instead.

rows() and cols() are constexpr for statically sized matrices, so
there's no performance loss there.
This commit is contained in:
Tyler Veness
2022-09-02 01:00:05 -07:00
committed by GitHub
parent c393b3b367
commit 0ef8a4e1df
2 changed files with 88 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
wpimath/src/main/native/include/frc/fmt/Eigen.h
View File

@@ -7,9 +7,10 @@
#include <fmt/format.h>
#include "Eigen/Core"
#include "Eigen/SparseCore"
/**
* Formatter for Eigen::Matrix<>.
* Formatter for Eigen::Matrix<double, Rows, Cols>.
*
* @tparam Rows Number of rows.
* @tparam Cols Number of columns.
@@ -51,12 +52,68 @@ struct fmt::formatter<Eigen::Matrix<double, Rows, Cols, Args...>> {
auto format(const Eigen::Matrix<double, Rows, Cols, Args...>& mat,
FormatContext& ctx) {
auto out = ctx.out();
for (int i = 0; i < Rows; ++i) {
for (int j = 0; j < Cols; ++j) {
for (int i = 0; i < mat.rows(); ++i) {
for (int j = 0; j < mat.cols(); ++j) {
out = fmt::format_to(out, " {:f}", mat(i, j));
}
if (i < Rows - 1) {
if (i < mat.rows() - 1) {
out = fmt::format_to(out, "\n");
}
}
return out;
}
};
/**
* Formatter for Eigen::SparseMatrix<double>.
*
* @tparam Options Union of bit flags controlling the storage scheme.
* @tparam StorageIndex The type of the indices.
*/
template <int Options, typename StorageIndex>
struct fmt::formatter<Eigen::SparseMatrix<double, Options, StorageIndex>> {
/**
* Storage for format specifier.
*/
char presentation = 'f';
/**
* Format string parser.
*
* @param ctx Format string context.
*/
constexpr auto parse(fmt::format_parse_context& ctx) {
auto it = ctx.begin(), end = ctx.end();
if (it != end && (*it == 'f' || *it == 'e')) {
presentation = *it++;
}
if (it != end && *it != '}') {
throw fmt::format_error("invalid format");
}
return it;
}
/**
* Writes out a formatted matrix.
*
* @tparam FormatContext Format string context type.
* @param mat Matrix to format.
* @param ctx Format string context.
*/
template <typename FormatContext>
auto format(const Eigen::SparseMatrix<double, Options, StorageIndex>& mat,
FormatContext& ctx) {
auto out = ctx.out();
for (int i = 0; i < mat.rows(); ++i) {
for (int j = 0; j < mat.cols(); ++j) {
out = fmt::format_to(out, " {:f}", mat.coeff(i, j));
}
if (i < mat.rows() - 1) {
out = fmt::format_to(out, "\n");
}
}

31
wpimath/src/test/native/cpp/FormatterTest.cpp
View File

@@ -2,6 +2,8 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <vector>
#include <fmt/format.h>
#include "frc/fmt/Eigen.h"
@@ -10,12 +12,33 @@
#include "units/velocity.h"
TEST(FormatterTest, Eigen) {
Eigen::Matrix<double, 3, 2> A{{1.0, 2.0}, {3.0, 4.0}, {5.0, 6.0}};
Eigen::Matrix<double, 3, 2> A{{0.0, 1.0}, {2.0, 3.0}, {4.0, 5.0}};
EXPECT_EQ(
" 1.000000 2.000000\n"
" 3.000000 4.000000\n"
" 5.000000 6.000000",
" 0.000000 1.000000\n"
" 2.000000 3.000000\n"
" 4.000000 5.000000",
fmt::format("{}", A));
Eigen::MatrixXd B{{0.0, 1.0}, {2.0, 3.0}, {4.0, 5.0}};
EXPECT_EQ(
" 0.000000 1.000000\n"
" 2.000000 3.000000\n"
" 4.000000 5.000000",
fmt::format("{}", B));
Eigen::SparseMatrix<double> C{3, 2};
std::vector<Eigen::Triplet<double>> triplets;
triplets.emplace_back(0, 1, 1.0);
triplets.emplace_back(1, 0, 2.0);
triplets.emplace_back(1, 1, 3.0);
triplets.emplace_back(2, 0, 4.0);
triplets.emplace_back(2, 1, 5.0);
C.setFromTriplets(triplets.begin(), triplets.end());
EXPECT_EQ(
" 0.000000 1.000000\n"
" 2.000000 3.000000\n"
" 4.000000 5.000000",
fmt::format("{}", C));
}
TEST(FormatterTest, Units) {