From 57ca59ddf04d40dd761cb550e65c30d497c647e7 Mon Sep 17 00:00:00 2001 From: Igor Bogoslavskyi Date: Tue, 27 Nov 2018 12:44:49 +0100 Subject: [PATCH] Fix Eigen point alignment in a vector --- depth_clustering.sublime-project | 67 +++++++++++++----------- src/projections/cloud_projection.cpp | 2 +- src/projections/cloud_projection.h | 14 ++--- src/projections/ring_projection.cpp | 4 +- src/projections/ring_projection.h | 5 +- src/projections/spherical_projection.cpp | 3 +- src/projections/spherical_projection.h | 3 +- src/utils/cloud.h | 5 +- src/utils/rich_point.h | 5 ++ 9 files changed, 61 insertions(+), 47 deletions(-) diff --git a/depth_clustering.sublime-project b/depth_clustering.sublime-project index 4b9dcff..3bb4331 100644 --- a/depth_clustering.sublime-project +++ b/depth_clustering.sublime-project @@ -1,32 +1,39 @@ { - "build_systems": - [ - { - "file_regex": "^[ ]*File \"(...*?)\", line ([0-9]*)", - "name": "Anaconda Python Builder", - "selector": "source.python", - "shell_cmd": "\"python\" -u \"$file\"" - } - ], - "folders": - [ - { - "path": "." - } - ], - "settings": - { - "ecc_common_flags": - [ - "-I/usr/include", - "-I/usr/lib/clang/$clang_version/include", - "-I~/Code/catkin_ws/build/$project_name/src" - ], - "ecc_flags_sources": [ - { - "file": "CMakeLists.txt", - "prefix_paths": ["/opt/ros/kinetic", "~/Code/catkin_ws/devel"] - } - ] - } + "build_systems": + [ + { + "file_regex": "^[ ]*File \"(...*?)\", line ([0-9]*)", + "name": "Anaconda Python Builder", + "selector": "source.python", + "shell_cmd": "\"python\" -u \"$file\"" + } + ], + "folders": + [ + { + "path": "." + } + ], + "settings": + { + "ecc_common_flags": + [ + "-I/usr/include", + "-I/usr/lib/clang/$clang_version/include", + "-I~/Code/catkin_ws/build/$project_name/src" + ], + "ecc_flags_sources": + [ + { + "file": "CMakeLists.txt", + "prefix_paths": + [ + "/opt/ros/kinetic", + "~/Code/catkin_ws/devel", + "/opt/ros/melodic" + ] + } + ], + "ecc_use_target_compiler_built_in_flags": false, + } } diff --git a/src/projections/cloud_projection.cpp b/src/projections/cloud_projection.cpp index 77fdde1..53fb2f6 100644 --- a/src/projections/cloud_projection.cpp +++ b/src/projections/cloud_projection.cpp @@ -47,7 +47,7 @@ RichPoint CloudProjection::UnprojectPoint(const cv::Mat& image, const int row, } void CloudProjection::CheckCloudAndStorage( - const std::vector& points) { + const RichPoint::AlignedVector& points) { if (this->_data.size() < 1) { throw std::length_error("_data size is < 1"); } diff --git a/src/projections/cloud_projection.h b/src/projections/cloud_projection.h index f59aee0..0492e54 100644 --- a/src/projections/cloud_projection.h +++ b/src/projections/cloud_projection.h @@ -57,7 +57,7 @@ class CloudProjection { * * @param[in] points The points */ - virtual void InitFromPoints(const std::vector& points) = 0; + virtual void InitFromPoints(const RichPoint::AlignedVector& points) = 0; /** * @brief Polymorphic clone of a projection. @@ -96,7 +96,7 @@ class CloudProjection { * * @param[in] points The points to check */ - void CheckCloudAndStorage(const std::vector& points); + void CheckCloudAndStorage(const RichPoint::AlignedVector& points); /** * @brief Unproject a point from depth image coordinate @@ -111,11 +111,11 @@ class CloudProjection { const int col) const; /** - * @brief Set corrections for systematic error in a dataset (see - * notebooks in the repo) - * - * @param[in] corrections A vector of correction in depth for every beam. - */ + * @brief Set corrections for systematic error in a dataset (see + * notebooks in the repo) + * + * @param[in] corrections A vector of correction in depth for every beam. + */ inline void SetCorrections(const std::vector& corrections) { _corrections = corrections; } diff --git a/src/projections/ring_projection.cpp b/src/projections/ring_projection.cpp index ad56244..067d482 100644 --- a/src/projections/ring_projection.cpp +++ b/src/projections/ring_projection.cpp @@ -21,9 +21,7 @@ namespace depth_clustering { -using std::vector; - -void RingProjection::InitFromPoints(const std::vector& points) { +void RingProjection::InitFromPoints(const RichPoint::AlignedVector& points) { fprintf(stderr, "Projecting cloud with %lu points\n", points.size()); time_utils::Timer timer; this->CheckCloudAndStorage(points); diff --git a/src/projections/ring_projection.h b/src/projections/ring_projection.h index 33fa83e..6a415ea 100644 --- a/src/projections/ring_projection.h +++ b/src/projections/ring_projection.h @@ -20,8 +20,9 @@ #include -#include "projections/projection_params.h" #include "projections/cloud_projection.h" +#include "projections/projection_params.h" +#include "utils/cloud.h" #include "utils/radians.h" namespace depth_clustering { @@ -39,7 +40,7 @@ class RingProjection : public CloudProjection { * * @param[in] cloud The cloud with laser ring information */ - void InitFromPoints(const std::vector& cloud) override; + void InitFromPoints(const RichPoint::AlignedVector& cloud) override; CloudProjection::Ptr Clone() const override; virtual ~RingProjection() {} diff --git a/src/projections/spherical_projection.cpp b/src/projections/spherical_projection.cpp index fa8a17e..b8ac0d2 100644 --- a/src/projections/spherical_projection.cpp +++ b/src/projections/spherical_projection.cpp @@ -19,7 +19,8 @@ namespace depth_clustering { -void SphericalProjection::InitFromPoints(const std::vector& points) { +void SphericalProjection::InitFromPoints( + const RichPoint::AlignedVector& points) { this->CheckCloudAndStorage(points); for (size_t index = 0; index < points.size(); ++index) { const auto& point = points[index]; diff --git a/src/projections/spherical_projection.h b/src/projections/spherical_projection.h index 3c76383..dcf6dc1 100644 --- a/src/projections/spherical_projection.h +++ b/src/projections/spherical_projection.h @@ -22,6 +22,7 @@ #include "projections/cloud_projection.h" #include "projections/projection_params.h" +#include "utils/cloud.h" namespace depth_clustering { @@ -38,7 +39,7 @@ class SphericalProjection : public CloudProjection { * * @param[in] points The points */ - void InitFromPoints(const std::vector& points) override; + void InitFromPoints(const RichPoint::AlignedVector& points) override; typename CloudProjection::Ptr Clone() const override; virtual ~SphericalProjection() {} }; diff --git a/src/utils/cloud.h b/src/utils/cloud.h index 3c39dbe..1f3b2bb 100644 --- a/src/utils/cloud.h +++ b/src/utils/cloud.h @@ -17,6 +17,7 @@ #define SRC_UTILS_CLOUD_H_ #include +#include #if PCL_FOUND #include @@ -53,7 +54,7 @@ class Cloud { virtual ~Cloud() {} - inline const std::vector& points() const { return _points; } + inline const RichPoint::AlignedVector& points() const { return _points; } inline Pose& pose() { return _pose; } inline const Pose& pose() const { return _pose; } @@ -109,7 +110,7 @@ class Cloud { #endif // PCL_FOUND protected: - std::vector _points = std::vector(); + RichPoint::AlignedVector _points; Pose _pose; Pose _sensor_pose; diff --git a/src/utils/rich_point.h b/src/utils/rich_point.h index 17ab3e4..86abdf9 100644 --- a/src/utils/rich_point.h +++ b/src/utils/rich_point.h @@ -17,6 +17,7 @@ #define SRC_UTILS_RICH_POINT_H_ #include +#include namespace depth_clustering { @@ -26,6 +27,10 @@ namespace depth_clustering { class RichPoint { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using AlignedVector = + std::vector>; + RichPoint() {} explicit RichPoint(float x, float y, float z) : _point(x, y, z) {} explicit RichPoint(float x, float y, float z, uint16_t ring)