aligned memory alloc using Eigen

lsd_slam_core/src/DataStructures/FrameMemory.cpp

@@ -51,7 +51,7 @@ void FrameMemory::releaseBuffes()
 
 		for(unsigned int i=0;i<p.second.size();i++)
 		{
-			delete (char*)p.second[i];
+			Eigen::internal::aligned_free(p.second[i]);
 			bufferSizes.erase(p.second[i]);
 		}
 
@@ -121,7 +121,7 @@ void* FrameMemory::allocateBuffer(unsigned int size)
 {
 	//printf("allocateFloatBuffer(%d)\n", size);
 
-	void* buffer = (void*)(new char[size]);
+	void* buffer = Eigen::internal::aligned_malloc(size);
 	bufferSizes.insert(std::make_pair(buffer, size));
 	return buffer;
 }

lsd_slam_core/src/DepthEstimation/DepthMap.cpp

@@ -48,7 +48,8 @@ DepthMap::DepthMap(int w, int h, const Eigen::Matrix3f& K)
 	otherDepthMap = new DepthMapPixelHypothesis[width*height];
 	currentDepthMap = new DepthMapPixelHypothesis[width*height];
 
-	validityIntegralBuffer =new int[width*height];
+	validityIntegralBuffer = (int*)Eigen::internal::aligned_malloc(width*height*sizeof(int));
+
 
 
 
@@ -94,9 +95,7 @@ DepthMap::~DepthMap()
 	delete[] otherDepthMap;
 	delete[] currentDepthMap;
 
-	delete[] validityIntegralBuffer;
-
-
+	Eigen::internal::aligned_free((void*)validityIntegralBuffer);
 }
 
 

lsd_slam_core/src/Tracking/SE3Tracker.cpp

@@ -26,6 +26,8 @@
 #include "IOWrapper/ImageDisplay.h"
 #include "Tracking/least_squares.h"
 
+#include <Eigen/Core>
+
 namespace lsd_slam
 {
 
@@ -62,16 +64,16 @@ SE3Tracker::SE3Tracker(int w, int h, Eigen::Matrix3f K)
 	cyi = KInv(1,2);
 
 
-	buf_warped_residual = new float[w*h];
-	buf_warped_dx = new float[w*h];
-	buf_warped_dy = new float[w*h];
-	buf_warped_x = new float[w*h];
-	buf_warped_y = new float[w*h];
-	buf_warped_z = new float[w*h];
+	buf_warped_residual = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_dx = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_dy = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_x = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_y = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_z = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
 
-	buf_d = new float[w*h];
-	buf_idepthVar = new float[w*h];
-	buf_weight_p = new float[w*h];
+	buf_d = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_idepthVar = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_weight_p = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
 
 	buf_warped_size = 0;
 
@@ -100,16 +102,17 @@ SE3Tracker::~SE3Tracker()
 	debugImageOldImageWarped.release();
 
 
-	delete[] buf_warped_residual;
-	delete[] buf_warped_dx;
-	delete[] buf_warped_dy;
-	delete[] buf_warped_x;
-	delete[] buf_warped_y;
-	delete[] buf_warped_z;
+	Eigen::internal::aligned_free((void*)buf_warped_residual);
+	Eigen::internal::aligned_free((void*)buf_warped_residual);
+	Eigen::internal::aligned_free((void*)buf_warped_dx);
+	Eigen::internal::aligned_free((void*)buf_warped_dy);
+	Eigen::internal::aligned_free((void*)buf_warped_x);
+	Eigen::internal::aligned_free((void*)buf_warped_y);
+	Eigen::internal::aligned_free((void*)buf_warped_z);
 
-	delete[] buf_d;
-	delete[] buf_idepthVar;
-	delete[] buf_weight_p;
+	Eigen::internal::aligned_free((void*)buf_d);
+	Eigen::internal::aligned_free((void*)buf_idepthVar);
+	Eigen::internal::aligned_free((void*)buf_weight_p);
 }
 
 

lsd_slam_core/src/Tracking/Sim3Tracker.cpp

@@ -63,24 +63,24 @@ Sim3Tracker::Sim3Tracker(int w, int h, Eigen::Matrix3f K)
 	cyi = KInv(1,2);
 
 
-	buf_warped_residual = new float[w*h];
-	buf_warped_weights = new float[w*h];
-	buf_warped_dx = new float[w*h];
-	buf_warped_dy = new float[w*h];
-	buf_warped_x = new float[w*h];
-	buf_warped_y = new float[w*h];
-	buf_warped_z = new float[w*h];
-
-	buf_d = new float[w*h];
-	buf_residual_d = new float[w*h];
-	buf_idepthVar = new float[w*h];
-	buf_warped_idepthVar = new float[w*h];
-	buf_weight_p = new float[w*h];
-	buf_weight_d = new float[w*h];
-
-	buf_weight_Huber = new float[w*h];
-	buf_weight_VarP = new float[w*h];
-	buf_weight_VarD = new float[w*h];
+	buf_warped_residual = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_weights = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_dx = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_dy = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_x = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_y = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_z = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+
+	buf_d = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_residual_d = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_idepthVar = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_warped_idepthVar = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_weight_p = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_weight_d = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+
+	buf_weight_Huber = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_weight_VarP = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
+	buf_weight_VarD = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));
 
 	buf_warped_size = 0;
 
@@ -125,24 +125,24 @@ Sim3Tracker::~Sim3Tracker()
 	debugImageWeightedResD.release();
 
 
-	delete[] buf_warped_residual;
-	delete[] buf_warped_weights;
-	delete[] buf_warped_dx;
-	delete[] buf_warped_dy;
-	delete[] buf_warped_x;
-	delete[] buf_warped_y;
-	delete[] buf_warped_z;
-
-	delete[] buf_d;
-	delete[] buf_residual_d;
-	delete[] buf_idepthVar;
-	delete[] buf_warped_idepthVar;
-	delete[] buf_weight_p;
-	delete[] buf_weight_d;
-
-	delete[] buf_weight_Huber;
-	delete[] buf_weight_VarP;
-	delete[] buf_weight_VarD;
+	Eigen::internal::aligned_free((void*)buf_warped_residual);
+	Eigen::internal::aligned_free((void*)buf_warped_weights);
+	Eigen::internal::aligned_free((void*)buf_warped_dx);
+	Eigen::internal::aligned_free((void*)buf_warped_dy);
+	Eigen::internal::aligned_free((void*)buf_warped_x);
+	Eigen::internal::aligned_free((void*)buf_warped_y);
+	Eigen::internal::aligned_free((void*)buf_warped_z);
+
+	Eigen::internal::aligned_free((void*)buf_d);
+	Eigen::internal::aligned_free((void*)buf_residual_d);
+	Eigen::internal::aligned_free((void*)buf_idepthVar);
+	Eigen::internal::aligned_free((void*)buf_warped_idepthVar);
+	Eigen::internal::aligned_free((void*)buf_weight_p);
+	Eigen::internal::aligned_free((void*)buf_weight_d);
+
+	Eigen::internal::aligned_free((void*)buf_weight_Huber);
+	Eigen::internal::aligned_free((void*)buf_weight_VarP);
+	Eigen::internal::aligned_free((void*)buf_weight_VarD);
 }
 
 

lsd_slam_core/src/Tracking/TrackingReference.cpp

@@ -50,7 +50,8 @@ void TrackingReference::releaseAll()
 		if(posData[level] != nullptr) delete[] posData[level];
 		if(gradData[level] != nullptr) delete[] gradData[level];
 		if(colorAndVarData[level] != nullptr) delete[] colorAndVarData[level];
-		if(pointPosInXYGrid[level] != nullptr) delete[] pointPosInXYGrid[level];
+		if(pointPosInXYGrid[level] != nullptr)
+			Eigen::internal::aligned_free((void*)pointPosInXYGrid[level]);
 		numData[level] = 0;
 	}
 	wh_allocated = 0;
@@ -114,7 +115,8 @@ void TrackingReference::makePointCloud(int level)
 	const Eigen::Vector4f* pyrGradSource = keyframe->gradients(level);
 
 	if(posData[level] == nullptr) posData[level] = new Eigen::Vector3f[w*h];
-	if(pointPosInXYGrid[level] == nullptr) pointPosInXYGrid[level] = new int[w*h];
+	if(pointPosInXYGrid[level] == nullptr)
+		pointPosInXYGrid[level] = (int*)Eigen::internal::aligned_malloc(w*h*sizeof(int));;
 	if(gradData[level] == nullptr) gradData[level] = new Eigen::Vector2f[w*h];
 	if(colorAndVarData[level] == nullptr) colorAndVarData[level] = new Eigen::Vector2f[w*h];
 

lsd_slam_core/src/util/Undistorter.cpp

@@ -25,6 +25,7 @@
 
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
+#include <Eigen/Core>
 #include "util/settings.h"
 
 namespace lsd_slam
@@ -278,8 +279,8 @@ UndistorterPTAM::UndistorterPTAM(const char* configFileName)
 		outputCalibration[3] = (ocy+0.5) / out_height;
 		outputCalibration[4] = 0;
 
-		remapX = new float[out_width * out_height];
-		remapY = new float[out_width * out_height];
+		remapX = (float*)Eigen::internal::aligned_malloc(out_width * out_height *sizeof(float));
+		remapY = (float*)Eigen::internal::aligned_malloc(out_width * out_height *sizeof(float));
 
 		for(int y=0;y<out_height;y++)
 		{
@@ -346,8 +347,8 @@ UndistorterPTAM::UndistorterPTAM(const char* configFileName)
 UndistorterPTAM::~UndistorterPTAM()
 {
 
-	delete[] remapX;
-	delete[] remapY;
+	Eigen::internal::aligned_free((void*)remapX);
+	Eigen::internal::aligned_free((void*)remapY);
 
 }
 

lsd_slam_core/src/util/globalFuncs.cpp

@@ -100,7 +100,8 @@ cv::Mat getDepthRainbowPlot(const float* idepth, const float* idepthVar, const f
 }
 cv::Mat getVarRedGreenPlot(const float* idepthVar, const float* gray, int width, int height)
 {
-	float* idepthVarExt = new float[width*height];
+	float* idepthVarExt = (float*)Eigen::internal::aligned_malloc(width*height*sizeof(float));
+
 	memcpy(idepthVarExt,idepthVar,sizeof(float)*width*height);
 
 	for(int i=2;i<width-2;i++)
@@ -157,7 +158,8 @@ cv::Mat getVarRedGreenPlot(const float* idepthVar, const float* gray, int width,
 			}
 		}
 
-	delete idepthVarExt;
+	Eigen::internal::aligned_free((void*)idepthVarExt);
+
 	return res;
 }
 }