Workaround for misalignment of SubmapSlice and Eigen data structures in heterogeneous builds

.gitignore

@@ -1,2 +1,4 @@
-build
+build*/
 bazel-*
+*.user
+

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

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<uint32_t> 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<uint32_t> 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<uint32_t> cairo_data;
-
-  // Metadata.
   ::cartographer::transform::Rigid3d pose;
-  int metadata_version = -1;
 };
 
 struct SubmapTexture {

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<Eigen::Vector2f> 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<Eigen::Vector2f> 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);
   }

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();) {

cartographer/mapping/pose_extrapolator.cc

@@ -18,7 +18,7 @@
 
 #include <algorithm>
 
-#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> PoseExtrapolator::InitializeWithImu(
     const common::Duration pose_queue_duration,
     const double imu_gravity_time_constant, const sensor::ImuData& imu_data) {
-  auto extrapolator = absl::make_unique<PoseExtrapolator>(
+  auto extrapolator = std::make_unique<PoseExtrapolator>(
       pose_queue_duration, imu_gravity_time_constant);
   extrapolator->AddImuData(imu_data);
   extrapolator->imu_tracker_ =
-      absl::make_unique<ImuTracker>(imu_gravity_time_constant, imu_data.time);
+      std::make_unique<ImuTracker>(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<ImuTracker>(gravity_time_constant_, tracker_start);
+        std::make_unique<ImuTracker>(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<ImuTracker>(*imu_tracker_);
-  extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
+  odometry_imu_tracker_ = std::make_unique<ImuTracker>(*imu_tracker_);
+  extrapolation_imu_tracker_ = std::make_unique<ImuTracker>(*imu_tracker_);
 }
 
 void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {

cartographer/sensor/timed_point_cloud_data.h

@@ -41,6 +41,12 @@ struct TimedPointCloudOriginData {
   common::Time time;
   std::vector<Eigen::Vector3f> origins;
   std::vector<RangeMeasurement> ranges;
+  TimedPointCloudOriginData(common::Time _time = common::Time(),
+                            std::vector<Eigen::Vector3f> _origins = std::vector<Eigen::Vector3f>(),
+                            std::vector<RangeMeasurement> _ranges = std::vector<RangeMeasurement>()):
+                                                                                                       time(_time),
+                                                                                                       origins(_origins),
+                                                                                                       ranges(_ranges) {}
 };
 
 // Converts 'timed_point_cloud_data' to a proto::TimedPointCloudData.

cartographer/transform/rigid_transform.h

@@ -28,6 +28,21 @@
 #include "cartographer/common/math.h"
 #include "cartographer/common/port.h"
 
+#include<Eigen/StdVector>
+/*
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<float, 2, 1>);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double, 2, 1>);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Rotation2D<float>);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Rotation2D<double>);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<float, 3, 1>);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double, 3, 1>);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Quaternion<float>);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Quaternion<double>);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::AngleAxis<float>);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::AngleAxis<double>);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double, 4, 1, 0, 4, 1>);
+*/
+
 namespace cartographer {
 namespace transform {