Skip to content

Commit

Permalink
less code, more readable
Browse files Browse the repository at this point in the history
  • Loading branch information
@facontidavide
facontidavide committed Dec 14, 2017
1 parent b8784a8 commit d9dcfa1
Show file tree
Hide file tree
Showing 2 changed files with 48 additions and 117 deletions.
7 changes: 7 additions & 0 deletions include/loam_velodyne/LaserMapping.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -155,6 +155,13 @@ class LaserMapping {


private:

int toIndex(int i, int j, int k) const
{
return i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k;
}


float _scanPeriod; ///< time per scan
const int _stackFrameNum;
const int _mapFrameNum;
Expand Down
158 changes: 41 additions & 117 deletions src/lib/LaserMapping.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ LaserMapping::LaserMapping(const float& scanPeriod,
_laserCloudCornerDSArray.resize(_laserCloudNum);
_laserCloudSurfDSArray.resize(_laserCloudNum);

for (int i = 0; i < _laserCloudNum; i++) {
for (size_t i = 0; i < _laserCloudNum; i++) {
_laserCloudCornerArray[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
_laserCloudSurfArray[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
_laserCloudCornerDSArray[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
Expand Down Expand Up @@ -506,161 +506,89 @@ void LaserMapping::process()
while (centerCubeI < 3) {
for (int j = 0; j < _laserCloudHeight; j++) {
for (int k = 0; k < _laserCloudDepth; k++) {
int i = _laserCloudWidth - 1;
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeCornerPointer =
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeSurfPointer =
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
for (; i >= 1; i--) {
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
_laserCloudCornerArray[i - 1 + _laserCloudWidth*j + _laserCloudWidth * _laserCloudHeight * k];
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
_laserCloudSurfArray[i - 1 + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
for (int i = _laserCloudWidth - 1; i >= 1; i--) {
std::swap( _laserCloudCornerArray[ toIndex(i, j, k) ],
_laserCloudCornerArray[ toIndex(i+1, j, k) ]);
std::swap( _laserCloudSurfArray[ toIndex(i, j, k) ],
_laserCloudSurfArray[ toIndex(i+1, j, k) ]);
}
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
laserCloudCubeCornerPointer;
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}

centerCubeI++;
_laserCloudCenWidth++;
}

while (centerCubeI >= _laserCloudWidth - 3) {
for (int j = 0; j < _laserCloudHeight; j++) {
for (int k = 0; k < _laserCloudDepth; k++) {
int i = 0;
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeCornerPointer =
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeSurfPointer =
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
for (; i < _laserCloudWidth - 1; i++) {
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
_laserCloudCornerArray[i + 1 + _laserCloudWidth*j + _laserCloudWidth * _laserCloudHeight * k];
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
_laserCloudSurfArray[i + 1 + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
for (int i = 0; i < _laserCloudWidth - 1; i++) {
std::swap(_laserCloudCornerArray[ toIndex(i, j, k) ],
_laserCloudCornerArray[ toIndex(i+1, j, k) ]);
std::swap(_laserCloudSurfArray[ toIndex(i, j, k) ],
_laserCloudSurfArray[ toIndex(i+1, j, k) ]);
}
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
laserCloudCubeCornerPointer;
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}

centerCubeI--;
_laserCloudCenWidth--;
}

while (centerCubeJ < 3) {
for (int i = 0; i < _laserCloudWidth; i++) {
for (int k = 0; k < _laserCloudDepth; k++) {
int j = _laserCloudHeight - 1;
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeCornerPointer =
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeSurfPointer =
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
for (; j >= 1; j--) {
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
_laserCloudCornerArray[i + _laserCloudWidth*(j - 1) + _laserCloudWidth * _laserCloudHeight*k];
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
_laserCloudSurfArray[i + _laserCloudWidth * (j - 1) + _laserCloudWidth * _laserCloudHeight*k];
for (int j = _laserCloudHeight - 1; j >= 1; j--) {
std::swap(_laserCloudCornerArray[ toIndex(i, j, k) ],
_laserCloudCornerArray[ toIndex(i, j-1, k) ]);
std::swap(_laserCloudSurfArray[ toIndex(i, j, k) ],
_laserCloudSurfArray[ toIndex(i, j-1, k) ]);
}
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
laserCloudCubeCornerPointer;
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}

centerCubeJ++;
_laserCloudCenHeight++;
}

while (centerCubeJ >= _laserCloudHeight - 3) {
for (int i = 0; i < _laserCloudWidth; i++) {
for (int k = 0; k < _laserCloudDepth; k++) {
int j = 0;
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeCornerPointer =
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeSurfPointer =
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
for (; j < _laserCloudHeight - 1; j++) {
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
_laserCloudCornerArray[i + _laserCloudWidth*(j + 1) + _laserCloudWidth * _laserCloudHeight*k];
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
_laserCloudSurfArray[i + _laserCloudWidth * (j + 1) + _laserCloudWidth * _laserCloudHeight*k];
for (int j = 0; j < _laserCloudHeight - 1; j++) {
std::swap(_laserCloudCornerArray[ toIndex(i, j, k) ],
_laserCloudCornerArray[ toIndex(i, j+1, k) ]);
std::swap(_laserCloudSurfArray[ toIndex(i, j, k) ],
_laserCloudSurfArray[ toIndex(i, j+1, k) ]);
}
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
laserCloudCubeCornerPointer;
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}

centerCubeJ--;
_laserCloudCenHeight--;
}

while (centerCubeK < 3) {
for (int i = 0; i < _laserCloudWidth; i++) {
for (int j = 0; j < _laserCloudHeight; j++) {
int k = _laserCloudDepth - 1;
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeCornerPointer =
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeSurfPointer =
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
for (; k >= 1; k--) {
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
_laserCloudCornerArray[i + _laserCloudWidth*j + _laserCloudWidth * _laserCloudHeight*(k - 1)];
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight*(k - 1)];
for (int k = _laserCloudDepth - 1; k >= 1; k--) {
std::swap(_laserCloudCornerArray[ toIndex(i, j, k) ],
_laserCloudCornerArray[ toIndex(i, j, k-1) ]);
std::swap(_laserCloudSurfArray[ toIndex(i, j, k) ],
_laserCloudSurfArray[ toIndex(i, j, k-1) ]);
}
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
laserCloudCubeCornerPointer;
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}

centerCubeK++;
_laserCloudCenDepth++;
}

while (centerCubeK >= _laserCloudDepth - 3) {
for (int i = 0; i < _laserCloudWidth; i++) {
for (int j = 0; j < _laserCloudHeight; j++) {
int k = 0;
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeCornerPointer =
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeSurfPointer =
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k];
for (; k < _laserCloudDepth - 1; k++) {
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
_laserCloudCornerArray[i + _laserCloudWidth*j + _laserCloudWidth * _laserCloudHeight*(k + 1)];
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight*(k + 1)];
for (int k = 0; k < _laserCloudDepth - 1; k++) {
std::swap(_laserCloudCornerArray[ toIndex(i, j, k) ],
_laserCloudCornerArray[ toIndex(i, j, k+1) ]);
std::swap(_laserCloudSurfArray[ toIndex(i, j, k) ],
_laserCloudSurfArray[ toIndex(i, j, k+1) ]);
}
_laserCloudCornerArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
laserCloudCubeCornerPointer;
_laserCloudSurfArray[i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}

centerCubeK--;
_laserCloudCenDepth--;
}
Expand Down Expand Up @@ -705,7 +633,7 @@ void LaserMapping::process()
}
}

size_t cubeIdx = i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k;
size_t cubeIdx = i + _laserCloudWidth*j + _laserCloudWidth * _laserCloudHeight * k;
if (isInLaserFOV) {
_laserCloudValidInd.push_back(cubeIdx);
}
Expand Down Expand Up @@ -872,26 +800,22 @@ void LaserMapping::optimizeTransformTobeMapped()
Vector3 vc(0,0,0);

for (int j = 0; j < 5; j++) {
vc.x() += _laserCloudCornerFromMap->points[pointSearchInd[j]].x;
vc.y() += _laserCloudCornerFromMap->points[pointSearchInd[j]].y;
vc.z() += _laserCloudCornerFromMap->points[pointSearchInd[j]].z;
vc += Vector3(_laserCloudCornerFromMap->points[pointSearchInd[j]]);
}
vc /= 5.0;

Eigen::Matrix3f mat_a;
mat_a.setZero();

for (int j = 0; j < 5; j++) {
float ax = _laserCloudCornerFromMap->points[pointSearchInd[j]].x - vc.x();
float ay = _laserCloudCornerFromMap->points[pointSearchInd[j]].y - vc.y();
float az = _laserCloudCornerFromMap->points[pointSearchInd[j]].z - vc.z();

mat_a(0,0) += ax * ax;
mat_a(0,1) += ax * ay;
mat_a(0,2) += ax * az;
mat_a(1,1) += ay * ay;
mat_a(1,2) += ay * az;
mat_a(2,2) += az * az;
Vector3 a = Vector3(_laserCloudCornerFromMap->points[pointSearchInd[j]]) - vc;

mat_a(0,0) += a.x() * a.x();
mat_a(0,1) += a.x() * a.y();
mat_a(0,2) += a.x() * a.z();
mat_a(1,1) += a.y() * a.y();
mat_a(1,2) += a.y() * a.z();
mat_a(2,2) += a.z() * a.z();
}
matA1 = mat_a / 5.0;

Expand Down

0 comments on commit d9dcfa1

Please sign in to comment.