Merge pull request #191 from jumonatr/aligned

Aligned Allocations

g2o/core/CMakeLists.txt

@@ -1,4 +1,5 @@
 ADD_LIBRARY(core ${G2O_LIB_TYPE}
+dynamic_aligned_buffer.hpp
 base_edge.h
 base_binary_edge.h          hyper_graph_action.cpp
 base_binary_edge.hpp        hyper_graph_action.h

g2o/core/base_multi_edge.h

@@ -36,6 +36,7 @@
 #include "base_edge.h"
 #include "robust_kernel.h"
 #include "g2o/config.h"
+#include "dynamic_aligned_buffer.hpp"
 
 namespace g2o {
 

g2o/core/base_multi_edge.hpp

@@ -74,6 +74,8 @@ void BaseMultiEdge<D, E>::linearizeOplus()
   ErrorVector errorBak;
   ErrorVector errorBeforeNumeric = _error;
 
+  dynamic_aligned_buffer<double> buffer{ 12 };
+
   for (size_t i = 0; i < _vertices.size(); ++i) {
     //Xi - estimate the jacobian numerically
     OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
@@ -83,11 +85,9 @@ void BaseMultiEdge<D, E>::linearizeOplus()
 
     const int vi_dim = vi->dimension();
     assert(vi_dim >= 0);
-#ifdef _MSC_VER
-    double* add_vi = new double[vi_dim];
-#else
-    double add_vi[vi_dim];
-#endif
+
+    double* add_vi = buffer.request(vi_dim);
+
     std::fill(add_vi, add_vi + vi_dim, 0.0);
     assert(_dimension >= 0);
     assert(_jacobianOplus[i].rows() == _dimension && _jacobianOplus[i].cols() == vi_dim && "jacobian cache dimension does not match");
@@ -110,9 +110,6 @@ void BaseMultiEdge<D, E>::linearizeOplus()
 
       _jacobianOplus[i].col(d) = scalar * errorBak;
     } // end dimension
-#ifdef _MSC_VER
-    delete[] add_vi;
-#endif
   }
   _error = errorBeforeNumeric;
 
@@ -266,6 +263,8 @@ void BaseMultiEdge<-1, E>::linearizeOplus()
   ErrorVector errorBak;
   ErrorVector errorBeforeNumeric = _error;
 
+  dynamic_aligned_buffer<double> buffer{ 12 };
+
   for (size_t i = 0; i < _vertices.size(); ++i) {
     //Xi - estimate the jacobian numerically
     OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
@@ -275,11 +274,9 @@ void BaseMultiEdge<-1, E>::linearizeOplus()
 
     const int vi_dim = vi->dimension();
     assert(vi_dim >= 0);
-#ifdef _MSC_VER
-    double* add_vi = new double[vi_dim];
-#else
-    double add_vi[vi_dim];
-#endif
+
+    double* add_vi = buffer.request(vi_dim);
+
     std::fill(add_vi, add_vi + vi_dim, 0.0);
     assert(_dimension >= 0);
     assert(_jacobianOplus[i].rows() == _dimension && _jacobianOplus[i].cols() == vi_dim && "jacobian cache dimension does not match");
@@ -302,9 +299,6 @@ void BaseMultiEdge<-1, E>::linearizeOplus()
 
       _jacobianOplus[i].col(d) = scalar * errorBak;
     } // end dimension
-#ifdef _MSC_VER
-    delete[] add_vi;
-#endif
   }
   _error = errorBeforeNumeric;
 

g2o/core/block_solver.hpp

@@ -25,6 +25,8 @@
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "sparse_optimizer.h"
+#include "dynamic_aligned_buffer.hpp"
+
 #include <Eigen/LU>
 #include <fstream>
 #include <iomanip>
@@ -70,8 +72,8 @@ void BlockSolver<Traits>::resize(int* blockPoseIndices, int numPoseBlocks,
   if (_doSchur) {
     // the following two are only used in schur
     assert(_sizePoses > 0 && "allocating with wrong size");
-    _coefficients = new double [s];
-    _bschur = new double[_sizePoses];
+    _coefficients = allocate_aligned<double>(s);
+    _bschur = allocate_aligned<double>(_sizePoses);
   }
 
   _Hpp=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
@@ -101,9 +103,9 @@ void BlockSolver<Traits>::deallocate()
   _Hschur=0;
   delete _DInvSchur;
   _DInvSchur=0;
-  delete[] _coefficients;
+  free_aligned(_coefficients);
   _coefficients = 0;
-  delete[] _bschur;
+  free_aligned(_bschur);
   _bschur = 0;
   delete _HplCCS;
   _HplCCS = 0;

g2o/core/dynamic_aligned_buffer.hpp

@@ -0,0 +1,75 @@
+#pragma once
+
+#include <cstddef>
+
+#include "Eigen/Core"
+
+namespace g2o
+{
+    // 16 byte aligned allocation functions
+    template<typename Type>
+    Type* allocate_aligned(size_t n)
+    {
+        return (Type*)Eigen::internal::aligned_malloc(n * sizeof(Type));
+    }
+
+    template<typename Type>
+    Type* reallocate_aligned(Type* ptr, size_t newSize, size_t oldSize)
+    {
+        return (Type*)Eigen::internal::aligned_realloc(ptr, newSize * sizeof(Type), oldSize * sizeof(Type));
+    }
+
+    template<typename Type>
+    void free_aligned(Type* block)
+    {
+
+        Eigen::internal::aligned_free(block);
+    }
+
+    template<typename Type>
+    struct dynamic_aligned_buffer
+    {
+        dynamic_aligned_buffer(size_t size)
+            : m_size{ 0 }, m_ptr{ nullptr }
+        {
+            allocate(size);
+        }
+
+        ~dynamic_aligned_buffer()
+        {
+            free();
+        }
+
+        Type* request(size_t n)
+        {
+            if (n <= m_size)
+                return m_ptr;
+
+            m_ptr = reallocate_aligned<Type>(m_ptr, n, m_size);
+            m_size = m_ptr ? n : 0;
+
+            return m_ptr;
+        }
+
+    private:
+        void allocate(size_t size)
+        {
+            m_ptr = allocate_aligned<Type>(size);
+            if (m_ptr != nullptr)
+                m_size = size;
+        }
+
+        void free()
+        {
+            if (m_ptr != nullptr)
+            {
+                free_aligned<Type>(m_ptr);
+                m_size = 0;
+                m_ptr = nullptr;
+            }
+        }
+
+        std::size_t m_size;
+        Type* m_ptr;
+    };
+}

g2o/core/solver.cpp

@@ -25,6 +25,7 @@
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "solver.h"
+#include "dynamic_aligned_buffer.hpp"
 
 #include <cstring>
 #include <algorithm>
@@ -39,8 +40,8 @@ Solver::Solver() :
 
 Solver::~Solver()
 {
-  delete[] _x;
-  delete[] _b;
+  free_aligned(_x);
+  free_aligned(_b);
 }
 
 void Solver::resizeVector(size_t sx)
@@ -50,18 +51,18 @@ void Solver::resizeVector(size_t sx)
   sx += _additionalVectorSpace; // allocate some additional space if requested
   if (_maxXSize < sx) {
     _maxXSize = 2*sx;
-    delete[] _x;
-    _x = new double[_maxXSize];
+    free_aligned(_x);
+    _x = allocate_aligned<double>(_maxXSize);
 #ifndef NDEBUG
     memset(_x, 0, _maxXSize * sizeof(double));
 #endif
     if (_b) { // backup the former b, might still be needed for online processing
       memcpy(_x, _b, oldSize * sizeof(double));
-      delete[] _b;
-      _b = new double[_maxXSize];
+      free_aligned(_b);
+      _b = allocate_aligned<double>(_maxXSize);
       std::swap(_b, _x);
     } else {
-      _b = new double[_maxXSize];
+      _b = allocate_aligned<double>(_maxXSize);
 #ifndef NDEBUG
       memset(_b, 0, _maxXSize * sizeof(double));
 #endif