Fix Eigen point alignment in a vector

PRBonn · Nov 27, 2018 · 57ca59d · 57ca59d
1 parent ed79668
commit 57ca59d
Show file tree

Hide file tree

Showing 9 changed files with 61 additions and 47 deletions.
diff --git 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
@@ -47,7 +47,7 @@ RichPoint CloudProjection::UnprojectPoint(const cv::Mat& image, const int row,
 }
 
 void CloudProjection::CheckCloudAndStorage(
-    const std::vector<RichPoint>& 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
@@ -57,7 +57,7 @@ class CloudProjection {
    *
    * @param[in]  points  The points
    */
-  virtual void InitFromPoints(const std::vector<RichPoint>& 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<RichPoint>& 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<float>& corrections) {
     _corrections = corrections;
   }

diff --git 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<RichPoint>& 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
@@ -20,8 +20,9 @@
 
 #include <vector>
 
-#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<RichPoint>& 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
@@ -19,7 +19,8 @@
 
 namespace depth_clustering {
 
-void SphericalProjection::InitFromPoints(const std::vector<RichPoint>& 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
@@ -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<RichPoint>& 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
@@ -17,6 +17,7 @@
 #define SRC_UTILS_CLOUD_H_
 
 #include <Eigen/Core>
+#include <Eigen/StdVector>
 
 #if PCL_FOUND
 #include <pcl/point_cloud.h>
@@ -53,7 +54,7 @@ class Cloud {
 
   virtual ~Cloud() {}
 
-  inline const std::vector<RichPoint>& 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<RichPoint> _points = std::vector<RichPoint>();
+  RichPoint::AlignedVector _points;
 
   Pose _pose;
   Pose _sensor_pose;

diff --git a/src/utils/rich_point.h b/src/utils/rich_point.h
@@ -17,6 +17,7 @@
 #define SRC_UTILS_RICH_POINT_H_
 
 #include <Eigen/Core>
+#include <vector>
 
 namespace depth_clustering {
 
@@ -26,6 +27,10 @@ namespace depth_clustering {
 class RichPoint {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  using AlignedVector =
+      std::vector<RichPoint, Eigen::aligned_allocator<RichPoint>>;
+
   RichPoint() {}
   explicit RichPoint(float x, float y, float z) : _point(x, y, z) {}
   explicit RichPoint(float x, float y, float z, uint16_t ring)