# Logo-beam source code analysis - imageproject CPP

LeGo-LOAM

A lightweight and ground optimized lidar odometry and mapping (LeGO-LOAM) system for ROS compatible UGVs. The system takes in point cloud from a Velodyne VLP-16 Lidar (placed horizontal) and optional IMU data as inputs. It outputs 6D pose estimation in real-time.

A brief introduction to logo-LOAM (laser SLAM, IMU+LiDAR) is given. Based on LOAM, it achieves the same accuracy and greatly reduces the amount of calculation. Using point cloud segmentation to distinguish noise, extract plane and edge features, use two-step LM optimization method to solve pose, and add loop detection to reduce drift.

Next, go straight to the code section:

github: https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

Logo-loam code structure

First, let's take a look at the code structure. It is very clear. cloud_msgs, launch, include, src (core program). Let's take a look at the following:

• CMakeList.txt

...
find_package(GTSAM REQUIRED QUIET)
find_package(PCL REQUIRED QUIET)
find_package(OpenCV REQUIRED QUIET)
...


Dependent libraries: GTSAM, PCL, OPenCV

Here is a brief introduction to GTSAM. GTSAM is a C++ library for smoothing and mapping in the field of robotics and computer vision. It uses factor graphs and Bayesian networks to maximize the posterior probability. If you want to learn more, you can see resources [2].

• launch/run.launch

Nodes: rviz, camera_init_to_map, base_link_to_camera, imageProjection, featureAssociation, mapOptmization, transformFusion

The main nodes are the last four, which are also the core programs under the corresponding src directory.

• include/utility.h

Define data structure: point cloud, topic, lidar parameters, image segmentation parameters, feature extraction parameters, optimization parameters, mapping parameters

...
/*
* A point cloud type that has "ring" channel
*/
struct PointXYZIR
{
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,
(float, x, x) (float, y, y)
(float, z, z) (float, intensity, intensity)
(uint16_t, ring, ring)
)
...


Here, we define a_ ID point cloud data type. velodyne lidar has this data, but other manufacturers may not have it; Different from laser_id, ring_id refers to the line number (0-15 for VLP-16) that is incremented from the lowest laser line.

...
// Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used

// VLP-16
extern const int N_SCAN = 16;//number of lasers
extern const int Horizon_SCAN = 1800;//number of points from one laser of one scan
extern const float ang_res_x = 0.2;//angle resolution of horizon
extern const float ang_res_y = 2.0;//angle resolution of vertical
extern const float ang_bottom = 15.0+0.1;
extern const int groundScanInd = 7;//Indicates the number of coils required on the ground;
...


The parameter settings of different laser radars should be adjusted according to their own actual conditions. If you use the data of a data set, such as KITTI, you need to adjust for HDL-64E. Moreover, because the vertical angle resolution of the radar is not uniform, you also need to write your own subsequent projection program.

Next, let's cut into the core program. Since there are four CPPs, in order not to be too long, let's analyze imageproject cpp.

• src/imageProjection.cpp

Overall process: subscribe to point cloud data callback processing - > point cloud conversion to pcl preprocessing - > intercept a frame of laser data - > projection mapping to image - > ground removal - > point cloud segmentation - > publish point cloud data - > Reset parameters;

Callback processing function subscribed to lidar data:

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
// 1. Convert ros message to pcl point cloud
copyPointCloud(laserCloudMsg);//Use pcl library function;
// 2. Start and end angle of a scan
findStartEndAngle();//1. the angle difference of one circle of data is calculated by atan2; 2. pay attention to the rationality of the range of calculation results
// 3. Range image projection
projectPointCloud();//Project the lidar data into a 16x1800 (depending on the radar angular resolution) point cloud array
// 4. Mark ground points
groundRemoval();//Calculate the pitch angle between the two lines according to the position of the points between the upper and lower lines. If it is less than 10 degrees, it is the ground point
// 5. Point cloud segmentation
cloudSegmentation();//Firstly, the point cloud is labeled by clustering, and the corresponding point cloud block is stored according to the label;
// 6. Publish all clouds
publishCloud();//Publish various point cloud data
// 7. Reset parameters for next iteration
resetParameters();
}


projectPointCloud(): Map 3D point cloud projection to 2D range image

// find the row and column index in the image for this point
if (useCloudRing == true){
rowIdn = laserCloudInRing->points[i].ring;
}else{
verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
rowIdn = (verticalAngle + ang_bottom) / ang_res_y;//Determine row index
}
horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;//Determine column index
range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);//Determine depth

Lidar coordinate system

atan2(y，x);

The angle of the arctangent is equal to the included angle between the X axis and the line passing through the origin and the given coordinate point (x, y). A positive result indicates the angle of counterclockwise rotation from the X axis, and a negative result indicates the angle of clockwise rotation from the X axis;

atan2 function

1. determine row index

Calculate according to ring id or coordinates:

θv=atan2(z/(x2+y2)1/2)\theta_v=atan2(z/(x^2+y^2)^{1/2})θv​=atan2(z/(x2+y2)1/2)

row=(θv+θbottom)/resolutionvrow = (\theta_v+\theta_{bottom})/resolution_vrow=(θv​+θbottom​)/resolutionv​

2. determine the column index ([− π, π][-\pi,\pi][− π, π] is converted to [01800][01800][01800]): (the front of the y axis is the middle dividing line)

θh=atan2(x/y)∗180/π\theta_h=atan2(x/y)*180/\piθh​=atan2(x/y)∗180/π

col=−round((θh−90)/resolutionh)+pointsizeoflaserperscanper/2col=-round((\theta_h-90)/resolution_h)+pointsizeoflaserperscanper/2col=−round((θh​−90)/resolutionh​)+pointsizeoflaserperscanper/2

3. determine the depth value: (pay attention to the effective range of the data)

range=(x2+y2+z2)(1/2)range=(x^2+y^2+z^2)^{(1/2)}range=(x2+y2+z2)(1/2)

groundRemoval(): ground removal

// groundMat
// -1, no valid info to check if ground of not
//  0, initial value, after validation, means not ground
//  1, ground
for (size_t j = 0; j < Horizon_SCAN; ++j){
for (size_t i = 0; i < groundScanInd; ++i){
//groundScanInd defines the number of laser lines hitting the ground
lowerInd = j + ( i )*Horizon_SCAN;
upperInd = j + (i+1)*Horizon_SCAN;

if (fullCloud->points[lowerInd].intensity == -1 ||
fullCloud->points[upperInd].intensity == -1){
// no info to check, invalid points
groundMat.at<int8_t>(i,j) = -1;
continue;
}

diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;

angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
//If the pitch angle of two adjacent points in the vertical direction is less than 10 degrees, it is determined as the ground point; Adjacent scanning circle
if (abs(angle - sensorMountAngle) <= 10){
groundMat.at<int8_t>(i,j) = 1;
groundMat.at<int8_t>(i+1,j) = 1;
}
}
}

Ground judgment principle

If the pitch angle of two vertical adjacent points is less than 10 degrees, it is determined as a ground point.

cloudSegmentation():

void cloudSegmentation(){
// segmentation process
for (size_t i = 0; i < N_SCAN; ++i)
for (size_t j = 0; j < Horizon_SCAN; ++j)
if (labelMat.at<int>(i,j) == 0)
labelComponents(i, j);//->Labelcomponents() clustering operation
...
}
//Local feature detection clustering
//Plane point and edge point
void labelComponents(int row, int col){
...
while(queueSize > 0){
// Pop point
fromIndX = queueIndX[queueStartInd];
fromIndY = queueIndY[queueStartInd];
--queueSize;
++queueStartInd;
// Mark popped point
labelMat.at<int>(fromIndX, fromIndY) = labelCount;
// Loop through all the neighboring grids of popped grid
//Upper, lower, left and right neighborhood points
for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
// new index
thisIndX = fromIndX + (*iter).first;
thisIndY = fromIndY + (*iter).second;
// index should be within the boundary
if (thisIndX < 0 || thisIndX >= N_SCAN)
continue;
// at range image margin (left or right side)
if (thisIndY < 0)
thisIndY = Horizon_SCAN - 1;
if (thisIndY >= Horizon_SCAN)
thisIndY = 0;
// prevent infinite loop (caused by put already examined point back)
if (labelMat.at<int>(thisIndX, thisIndY) != 0)
continue;
//Compare larger depth with smaller depth
d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY),
rangeMat.at<float>(thisIndX, thisIndY));
d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY),
rangeMat.at<float>(thisIndX, thisIndY));

if ((*iter).first == 0)
alpha = segmentAlphaX;
else
alpha = segmentAlphaY;

angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));//Plane clustering formula: the larger the angle, the more the two points tend to be plane
//segmentTheta=60 degrees
if (angle > segmentTheta){

queueIndX[queueEndInd] = thisIndX;
queueIndY[queueEndInd] = thisIndY;
++queueSize;
++queueEndInd;

labelMat.at<int>(thisIndX, thisIndY) = labelCount;
lineCountFlag[thisIndX] = true;

allPushedIndX[allPushedIndSize] = thisIndX;
allPushedIndY[allPushedIndSize] = thisIndY;
++allPushedIndSize;
}
}
}

// check if this segment is valid
bool feasibleSegment = false;
if (allPushedIndSize >= 30)
feasibleSegment = true;//Pastry
else if (allPushedIndSize >= segmentValidPointNum){
int lineCount = 0;
for (size_t i = 0; i < N_SCAN; ++i)
if (lineCountFlag[i] == true)
++lineCount;
if (lineCount >= segmentValidLineNum)
feasibleSegment = true;//Edge point
}
// segment is valid, mark these points
if (feasibleSegment == true){
++labelCount;
}else{ // segment is invalid, mark these points
for (size_t i = 0; i < allPushedIndSize; ++i){
labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
}
}
}


Take the labelMat with the ground removed, calculate 4 points in the neighborhood point by point (pay attention to the effective range of the neighborhood index), compare the maximum value and the minimum value of the distance with the center point, and combine the threshold π /3\pi/3 π /3 through the following formula. If the value is greater than this value, the plane feature will be formed as a plane point. The array stores the index, labelMat stores the label value, and lineCountFlag is set to true until the threshold is not met, jump out and proceed to the next step.

Plane judgment principle

Clustering, more than 30 points are clustered into a class, labelCount++; If it is less than 30 and more than 5, the number of cluster points in the vertical direction will be counted, and if it is more than 3, it will also be marked as a class; If none is satisfied, the value 999999 indicates the cluster points to be discarded.

publishCloud(): point cloud Publishing

resetParameters(): parameter reset

References:

[1]. Shan T, Englot B. Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 4758-4765.

[6].atan2

Tags: slam

Posted by deko on Tue, 31 May 2022 05:46:42 +0530