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/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 { 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 {