From c26604ae478f40c677b100ab9dd48d2ea179aeef Mon Sep 17 00:00:00 2001 From: Stefan Fabian Date: Tue, 10 Dec 2024 10:18:00 +0100 Subject: [PATCH] Added eigen iterator to automatically determine optimal iteration order (row/col major). --- hector_math/benchmark/iterators.cpp | 66 +++++++++++++++++-- hector_math/include/hector_math/iterators.h | 12 ++++ .../hector_math/iterators/eigen_iterator.h | 35 ++++++++++ .../iterators/rectangle_iterator.h | 5 +- 4 files changed, 109 insertions(+), 9 deletions(-) create mode 100644 hector_math/include/hector_math/iterators.h create mode 100644 hector_math/include/hector_math/iterators/eigen_iterator.h diff --git a/hector_math/benchmark/iterators.cpp b/hector_math/benchmark/iterators.cpp index 063736d..d36cbb7 100644 --- a/hector_math/benchmark/iterators.cpp +++ b/hector_math/benchmark/iterators.cpp @@ -2,6 +2,7 @@ // Licensed under the MIT license. See LICENSE file in the project root for full license information. #include "hector_math/iterators/circle_iterator.h" +#include "hector_math/iterators/eigen_iterator.h" #include "hector_math/iterators/polygon_iterator.h" #include "hector_math/iterators/rectangle_iterator.h" #include "iterators_input.h" @@ -20,12 +21,12 @@ using namespace hector_math; template static void rectangleIterator( benchmark::State &state ) { - GridMap map( 20, 20 ); + GridMap map( 200, 200 ); for ( auto _ : state ) { // See test_hector_iterators for what this will iterate - iterateRectangle( Vector2( 0, 1 ), Vector2( 1, 19 ), - Vector2( 18, 0 ), + iterateRectangle( Vector2( 0, 1 ), Vector2( 1, 190 ), + Vector2( 180, 0 ), [&map]( Eigen::Index x, Eigen::Index y ) { ++map( x, y ); } ); } } @@ -36,10 +37,10 @@ template static void polygonIterator( benchmark::State &state ) { Polygon polygon = createPolygon(); - GridMap map( 20, 20 ); + GridMap map( 200, 200 ); for ( auto _ : state ) { - iteratePolygon( polygon / Scalar( 0.05 ), + iteratePolygon( polygon / Scalar( 0.005 ), [&map]( Eigen::Index x, Eigen::Index y ) { ++map( x, y ); } ); } } @@ -71,10 +72,10 @@ BENCHMARK( comparisonGridmapPolygonIterator )->Unit( benchmark::kMicrosecond ); template static void circleIterator( benchmark::State &state ) { - GridMap map( 20, 20 ); + GridMap map( 200, 200 ); for ( auto _ : state ) { - iterateCircle( Vector2( 10, 10 ), 10, + iterateCircle( Vector2( 100, 100 ), 100, [&map]( Eigen::Index x, Eigen::Index y ) { ++map( x, y ); } ); } } @@ -100,4 +101,55 @@ static void comparisonGridmapCircleIterator( benchmark::State &state ) BENCHMARK( comparisonGridmapCircleIterator )->Unit( benchmark::kMicrosecond ); #endif +static void eigenIteratorRowMajor( benchmark::State &state ) +{ + Eigen::Matrix map( state.range( 0 ), + state.range( 0 ) ); + map.setRandom(); + + for ( auto _ : state ) { + float sum = 0; + iterateDenseBase( map, [&map, &sum]( Eigen::Index x, Eigen::Index y ) { sum += map( x, y ); } ); + benchmark::DoNotOptimize( sum ); + } +} + +static void eigenIteratorColMajor( benchmark::State &state ) +{ + Eigen::Matrix map( state.range( 0 ), + state.range( 0 ) ); + map.setRandom(); + + for ( auto _ : state ) { + float sum = 0; + iterateDenseBase( map, [&map, &sum]( Eigen::Index x, Eigen::Index y ) { sum += map( x, y ); } ); + benchmark::DoNotOptimize( sum ); + } +} + +template +static void eigenBaseRowCol( benchmark::State &state ) +{ + Eigen::Matrix map( state.range( 0 ), + state.range( 0 ) ); + map.setRandom(); + + for ( auto _ : state ) { + float sum = 0; + for ( Eigen::Index row = 0; row < map.rows(); ++row ) { + for ( Eigen::Index col = 0; col < map.cols(); ++col ) { sum += map( row, col ); } + } + benchmark::DoNotOptimize( sum ); + } +} + +BENCHMARK( eigenIteratorRowMajor )->Unit( benchmark::kMicrosecond )->Arg( 100 ); +BENCHMARK( eigenIteratorColMajor )->Unit( benchmark::kMicrosecond )->Arg( 100 ); +BENCHMARK_TEMPLATE( eigenBaseRowCol, Eigen::RowMajor )->Unit( benchmark::kMicrosecond )->Arg( 100 ); +BENCHMARK_TEMPLATE( eigenBaseRowCol, Eigen::ColMajor )->Unit( benchmark::kMicrosecond )->Arg( 100 ); +BENCHMARK( eigenIteratorRowMajor )->Unit( benchmark::kMicrosecond )->Arg( 10000 ); +BENCHMARK( eigenIteratorColMajor )->Unit( benchmark::kMicrosecond )->Arg( 10000 ); +BENCHMARK_TEMPLATE( eigenBaseRowCol, Eigen::RowMajor )->Unit( benchmark::kMicrosecond )->Arg( 10000 ); +BENCHMARK_TEMPLATE( eigenBaseRowCol, Eigen::ColMajor )->Unit( benchmark::kMicrosecond )->Arg( 10000 ); + BENCHMARK_MAIN(); diff --git a/hector_math/include/hector_math/iterators.h b/hector_math/include/hector_math/iterators.h new file mode 100644 index 0000000..486d2de --- /dev/null +++ b/hector_math/include/hector_math/iterators.h @@ -0,0 +1,12 @@ +// Copyright (c) 2024 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license information. + +#ifndef HECTOR_MATH_ITERATORS_H +#define HECTOR_MATH_ITERATORS_H + +#include "hector_math/iterators/circle_iterator.h" +#include "hector_math/iterators/eigen_iterator.h" +#include "hector_math/iterators/polygon_iterator.h" +#include "hector_math/iterators/rectangle_iterator.h" + +#endif // HECTOR_MATH_ITERATORS_H diff --git a/hector_math/include/hector_math/iterators/eigen_iterator.h b/hector_math/include/hector_math/iterators/eigen_iterator.h new file mode 100644 index 0000000..91f32c9 --- /dev/null +++ b/hector_math/include/hector_math/iterators/eigen_iterator.h @@ -0,0 +1,35 @@ +// Copyright (c) 2024 Stefan Fabian. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license information. + +#ifndef HECTOR_MATH_EIGEN_ITERATOR_H +#define HECTOR_MATH_EIGEN_ITERATOR_H + +#include "hector_math/types.h" + +namespace hector_math +{ +/*! + * Convenience function to iterate over all elements of a dense Eigen matrix or array in the most efficient fashion. + * + * + * @tparam Functor A function or lambda method with the signature: void(Eigen::Index x, Eigen::Index y). + * @param functor The function that will be called for each index (x, y) inside the polygon. + */ +template +void iterateDenseBase( const Eigen::DenseBase &map, Functor functor ) +{ + const Eigen::Index cols = map.cols(); + const Eigen::Index rows = map.rows(); + if ( !Eigen::DenseBase::IsRowMajor ) { + for ( Eigen::Index y = 0; y < cols; ++y ) { + for ( Eigen::Index x = 0; x < rows; ++x ) { functor( x, y ); } + } + } else { + for ( Eigen::Index x = 0; x < rows; ++x ) { + for ( Eigen::Index y = 0; y < cols; ++y ) { functor( x, y ); } + } + } +} +} // namespace hector_math + +#endif // HECTOR_MATH_EIGEN_ITERATOR_H diff --git a/hector_math/include/hector_math/iterators/rectangle_iterator.h b/hector_math/include/hector_math/iterators/rectangle_iterator.h index 33e0a1a..4895551 100644 --- a/hector_math/include/hector_math/iterators/rectangle_iterator.h +++ b/hector_math/include/hector_math/iterators/rectangle_iterator.h @@ -57,7 +57,7 @@ void iterateRectangle( const Vector2 &a, const Vector2 &b, const Vector2, 4> points = { a, b, d, c }; // Find the corner with the lowest y value @@ -106,7 +106,8 @@ void iterateRectangle( const Vector2 &a, const Vector2 &b, const Vector2( col_max, std::round( highest.y() ) ); const Eigen::Index left_switch = std::round( left.y() ); const Eigen::Index right_switch = std::round( right.y() ); - Line left_line( lowest, left, y, true ), right_line( lowest, right, y, false ); + Line left_line( lowest, left, y, true ); + Line right_line( lowest, right, y, false ); Eigen::Index next_y = std::min( left_switch, right_switch ); // Loop until next corner for ( ; y < next_y; ++y ) {