From a1b6a27a16cd68b78d1b79a8e3eb3ca2832663e4 Mon Sep 17 00:00:00 2001 From: Andrey Vukolov Date: Fri, 11 Nov 2022 16:39:23 +0100 Subject: [PATCH 1/2] Workaround for misalignment in heterogeneous builds - Added a workaround for misaligned SubmapSlice structure between cartographer and cartographer_ros built using different compilers or compile contexts - Rearranged SubmapSlice field order after static analysis performed with clangd - Added commented non-portable workaround with \#pragma pack - Closes #1909 Signed-off-by: Andrey Vukolov --- cartographer/io/submap_painter.h | 55 ++++++++++++++++++++++++++------ 1 file changed, 45 insertions(+), 10 deletions(-) diff --git a/cartographer/io/submap_painter.h b/cartographer/io/submap_painter.h index f793494f9a..81824f0206 100644 --- a/cartographer/io/submap_painter.h +++ b/cartographer/io/submap_painter.h @@ -26,6 +26,10 @@ #include "cartographer/mapping/value_conversion_tables.h" #include "cartographer/transform/rigid_transform.h" +// Workaround to prevent SubmapSlice structure alignment issue with cartographer_ros +// Github issue #1909 +// #pragma pack(16) + namespace cartographer { namespace io { @@ -39,23 +43,54 @@ struct PaintSubmapSlicesResult { Eigen::Array2f origin; }; -struct SubmapSlice { +/* Structure rearranged according to the static analyzer recommendation + * SA Message: + * Excessive padding in 'struct cartographer::io::SubmapSlice' (32 padding bytes, + * where 0 is optimal). Optimal fields order: + * - slice_pose, + * - pose, + * - resolution, + * - surface, + * - cairo_data, + * - width, + * - height, + * - version, + * - metadata_version, + * consider reordering the fields or adding explicit padding members + * [clang-analyzer-optin.performance.Padding] + * + * Old version: + * // Texture data. + * int width; + * int height; + * int version; + * double resolution; + * ::cartographer::transform::Rigid3d slice_pose; + * ::cartographer::io::UniqueCairoSurfacePtr surface; + * // Pixel data used by 'surface'. Must outlive 'surface'. + * std::vector cairo_data; + * // Metadata. + * ::cartographer::transform::Rigid3d pose; + * int metadata_version = -1; +*/ + +// Another C++11-compliant workaround to prevent SubmapSlice structure +// alignment issue with cartographer_ros +// Github issue #1909 +struct alignas(32) SubmapSlice { + // Pixel data used by 'surface'. Must outlive 'surface'. + std::vector cairo_data; + ::cartographer::io::UniqueCairoSurfacePtr surface; SubmapSlice() : surface(::cartographer::io::MakeUniqueCairoSurfacePtr(nullptr)) {} - - // Texture data. + double resolution; + // Metadata. int width; int height; int version; - double resolution; + int metadata_version = -1; ::cartographer::transform::Rigid3d slice_pose; - ::cartographer::io::UniqueCairoSurfacePtr surface; - // Pixel data used by 'surface'. Must outlive 'surface'. - std::vector cairo_data; - - // Metadata. ::cartographer::transform::Rigid3d pose; - int metadata_version = -1; }; struct SubmapTexture { From 0c62d34d2fcd9481f3766106c8e390482390f288 Mon Sep 17 00:00:00 2001 From: Andrey Vukolov Date: Mon, 14 Nov 2022 17:27:49 +0100 Subject: [PATCH 2/2] Adoption for C++17 standard + Eigen workaround - Adopted C++17 standard, removed CXX standard requirement - Removed Abseil's absl::unique_ptr in favour of std::unique_ptr producing errors when built on Ubuntu 22 with fresh Abseil built from source - Added DISABLE_AVX_ALIGNMENT CMake option to prevent Eigen (especially built from source) from misalignment of the data structures passed outside the library namespace on the machines with modern CPUs providing AVX vectorization out of the box - Tested on Intel Core-i7 6700, any performance decreaseing was not discovered Signed-off-by: Andrey Vukolov --- .gitignore | 4 +++- CMakeLists.txt | 4 ++++ .../2d/scan_matching/interpolated_tsdf_2d_test.cc | 8 +++++--- .../mapping/internal/range_data_collator.cc | 4 ++-- cartographer/mapping/pose_extrapolator.cc | 12 ++++++------ cartographer/sensor/timed_point_cloud_data.h | 6 ++++++ cartographer/transform/rigid_transform.h | 15 +++++++++++++++ 7 files changed, 41 insertions(+), 12 deletions(-) diff --git a/.gitignore b/.gitignore index d1f961a8ff..e28025a94e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ -build +build*/ bazel-* +*.user + diff --git a/CMakeLists.txt b/CMakeLists.txt index ef2fcb3e69..f6f66fd422 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,7 @@ set(CARTOGRAPHER_SOVERSION ${CARTOGRAPHER_MAJOR_VERSION}.${CARTOGRAPHER_MINOR_VE option(BUILD_GRPC "build Cartographer gRPC support" false) set(CARTOGRAPHER_HAS_GRPC ${BUILD_GRPC}) option(BUILD_PROMETHEUS "build Prometheus monitoring support" false) +option(DISABLE_AVX_ALIGNMENT "disable AVX x32 Eigen-containing structure alignment producing segmentation faults on AVX-enabled systems" false) include("${PROJECT_SOURCE_DIR}/cmake/functions.cmake") google_initialize_cartographer_project() @@ -185,6 +186,9 @@ endif() set(INSTALL_GENERATED_HDRS ${ALL_PROTO_HDRS} ${ALL_GRPC_SERVICE_HDRS}) add_library(${PROJECT_NAME} STATIC ${ALL_LIBRARY_HDRS} ${ALL_LIBRARY_SRCS}) +if(DISABLE_AVX_ALIGNMENT) + target_compile_definitions(${PROJECT_NAME} PUBLIC EIGEN_MAX_ALIGN_BYTES=16) +endif(DISABLE_AVX_ALIGNMENT) configure_file( ${PROJECT_SOURCE_DIR}/cartographer/common/config.h.cmake diff --git a/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d_test.cc index c1f4fa6710..6fb257caa2 100644 --- a/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d_test.cc @@ -46,9 +46,11 @@ class InterpolatedTSDF2DTest : public ::testing::Test { }; TEST_F(InterpolatedTSDF2DTest, InterpolatesGridPoints) { - std::vector inner_points = { - Eigen::Vector2f(1.f, 1.f), Eigen::Vector2f(2.f, 1.f), - Eigen::Vector2f(1.f, 2.f), Eigen::Vector2f(2.f, 2.f)}; + std::vector inner_points; + inner_points.push_back(Eigen::Vector2f(1.f, 1.f)); + inner_points.push_back(Eigen::Vector2f(2.f, 1.f)); + inner_points.push_back(Eigen::Vector2f(1.f, 2.f)); + inner_points.push_back(Eigen::Vector2f(2.f, 2.f)); for (const Eigen::Vector2f& point : inner_points) { tsdf_.SetCell(tsdf_.limits().GetCellIndex(point), 0.1f, 1.0f); } diff --git a/cartographer/mapping/internal/range_data_collator.cc b/cartographer/mapping/internal/range_data_collator.cc index 3ce9e2145d..5d73e66b89 100644 --- a/cartographer/mapping/internal/range_data_collator.cc +++ b/cartographer/mapping/internal/range_data_collator.cc @@ -45,7 +45,7 @@ sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData( } id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data)); if (expected_sensor_ids_.size() != id_to_pending_data_.size()) { - return {}; + return sensor::TimedPointCloudOriginData(); } current_start_ = current_end_; // We have messages from all sensors, move forward to oldest. @@ -58,7 +58,7 @@ sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData( } sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { - sensor::TimedPointCloudOriginData result{current_end_, {}, {}}; + sensor::TimedPointCloudOriginData result(current_end_); bool warned_for_dropped_points = false; for (auto it = id_to_pending_data_.begin(); it != id_to_pending_data_.end();) { diff --git a/cartographer/mapping/pose_extrapolator.cc b/cartographer/mapping/pose_extrapolator.cc index 24e3800887..bbd1312f45 100644 --- a/cartographer/mapping/pose_extrapolator.cc +++ b/cartographer/mapping/pose_extrapolator.cc @@ -18,7 +18,7 @@ #include -#include "absl/memory/memory.h" +//#include "absl/memory/memory.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -35,11 +35,11 @@ PoseExtrapolator::PoseExtrapolator(const common::Duration pose_queue_duration, std::unique_ptr PoseExtrapolator::InitializeWithImu( const common::Duration pose_queue_duration, const double imu_gravity_time_constant, const sensor::ImuData& imu_data) { - auto extrapolator = absl::make_unique( + auto extrapolator = std::make_unique( pose_queue_duration, imu_gravity_time_constant); extrapolator->AddImuData(imu_data); extrapolator->imu_tracker_ = - absl::make_unique(imu_gravity_time_constant, imu_data.time); + std::make_unique(imu_gravity_time_constant, imu_data.time); extrapolator->imu_tracker_->AddImuLinearAccelerationObservation( imu_data.linear_acceleration); extrapolator->imu_tracker_->AddImuAngularVelocityObservation( @@ -73,7 +73,7 @@ void PoseExtrapolator::AddPose(const common::Time time, tracker_start = std::min(tracker_start, imu_data_.front().time); } imu_tracker_ = - absl::make_unique(gravity_time_constant_, tracker_start); + std::make_unique(gravity_time_constant_, tracker_start); } timed_pose_queue_.push_back(TimedPose{time, pose}); while (timed_pose_queue_.size() > 2 && @@ -84,8 +84,8 @@ void PoseExtrapolator::AddPose(const common::Time time, AdvanceImuTracker(time, imu_tracker_.get()); TrimImuData(); TrimOdometryData(); - odometry_imu_tracker_ = absl::make_unique(*imu_tracker_); - extrapolation_imu_tracker_ = absl::make_unique(*imu_tracker_); + odometry_imu_tracker_ = std::make_unique(*imu_tracker_); + extrapolation_imu_tracker_ = std::make_unique(*imu_tracker_); } void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) { diff --git a/cartographer/sensor/timed_point_cloud_data.h b/cartographer/sensor/timed_point_cloud_data.h index 8bdd928027..efe857a269 100644 --- a/cartographer/sensor/timed_point_cloud_data.h +++ b/cartographer/sensor/timed_point_cloud_data.h @@ -41,6 +41,12 @@ struct TimedPointCloudOriginData { common::Time time; std::vector origins; std::vector ranges; + TimedPointCloudOriginData(common::Time _time = common::Time(), + std::vector _origins = std::vector(), + std::vector _ranges = std::vector()): + time(_time), + origins(_origins), + ranges(_ranges) {} }; // Converts 'timed_point_cloud_data' to a proto::TimedPointCloudData. diff --git a/cartographer/transform/rigid_transform.h b/cartographer/transform/rigid_transform.h index 0823037744..6e16eb92d9 100644 --- a/cartographer/transform/rigid_transform.h +++ b/cartographer/transform/rigid_transform.h @@ -28,6 +28,21 @@ #include "cartographer/common/math.h" #include "cartographer/common/port.h" +#include +/* +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix); +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix); +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Rotation2D); +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Rotation2D); +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix); +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix); +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Quaternion); +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Quaternion); +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::AngleAxis); +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::AngleAxis); +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix); +*/ + namespace cartographer { namespace transform {