diff --git a/g2o/core/CMakeLists.txt b/g2o/core/CMakeLists.txt index 9ca409569..0512d3789 100644 --- a/g2o/core/CMakeLists.txt +++ b/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 diff --git a/g2o/core/base_multi_edge.h b/g2o/core/base_multi_edge.h index 68dbe2b18..e18d93ef6 100644 --- a/g2o/core/base_multi_edge.h +++ b/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 { diff --git a/g2o/core/base_multi_edge.hpp b/g2o/core/base_multi_edge.hpp index f9772c082..72c9e75a9 100644 --- a/g2o/core/base_multi_edge.hpp +++ b/g2o/core/base_multi_edge.hpp @@ -74,6 +74,8 @@ void BaseMultiEdge::linearizeOplus() ErrorVector errorBak; ErrorVector errorBeforeNumeric = _error; + dynamic_aligned_buffer buffer{ 12 }; + for (size_t i = 0; i < _vertices.size(); ++i) { //Xi - estimate the jacobian numerically OptimizableGraph::Vertex* vi = static_cast(_vertices[i]); @@ -83,11 +85,9 @@ void BaseMultiEdge::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::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 buffer{ 12 }; + for (size_t i = 0; i < _vertices.size(); ++i) { //Xi - estimate the jacobian numerically OptimizableGraph::Vertex* vi = static_cast(_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; diff --git a/g2o/core/block_solver.hpp b/g2o/core/block_solver.hpp index 64ae8cfa4..624917799 100644 --- a/g2o/core/block_solver.hpp +++ b/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 #include #include @@ -70,8 +72,8 @@ void BlockSolver::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(s); + _bschur = allocate_aligned(_sizePoses); } _Hpp=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks); @@ -101,9 +103,9 @@ void BlockSolver::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; diff --git a/g2o/core/dynamic_aligned_buffer.hpp b/g2o/core/dynamic_aligned_buffer.hpp new file mode 100644 index 000000000..c1727b7be --- /dev/null +++ b/g2o/core/dynamic_aligned_buffer.hpp @@ -0,0 +1,75 @@ +#pragma once + +#include + +#include "Eigen/Core" + +namespace g2o +{ + // 16 byte aligned allocation functions + template + Type* allocate_aligned(size_t n) + { + return (Type*)Eigen::internal::aligned_malloc(n * sizeof(Type)); + } + + template + 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 + void free_aligned(Type* block) + { + + Eigen::internal::aligned_free(block); + } + + template + 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(m_ptr, n, m_size); + m_size = m_ptr ? n : 0; + + return m_ptr; + } + + private: + void allocate(size_t size) + { + m_ptr = allocate_aligned(size); + if (m_ptr != nullptr) + m_size = size; + } + + void free() + { + if (m_ptr != nullptr) + { + free_aligned(m_ptr); + m_size = 0; + m_ptr = nullptr; + } + } + + std::size_t m_size; + Type* m_ptr; + }; +} \ No newline at end of file diff --git a/g2o/core/solver.cpp b/g2o/core/solver.cpp index cdddfac0e..f39c564c8 100644 --- a/g2o/core/solver.cpp +++ b/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 #include @@ -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(_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(_maxXSize); std::swap(_b, _x); } else { - _b = new double[_maxXSize]; + _b = allocate_aligned(_maxXSize); #ifndef NDEBUG memset(_b, 0, _maxXSize * sizeof(double)); #endif