src/featureAssociation.cpp
訂閱分割點雲、外點、imu資料，總體流程：（特征提取部分）訂閱傳感器資料->運動畸變去除->曲率計算->去除瑕點->提取邊、平面特征->釋出特征點雲；（特征關聯部分）将IMU資訊作為先驗->根據線特征、面特征計算位姿變換->聯合IMU資料進行位姿更新->釋出裡程計資訊
...
/**
1. Feature Extraction
*/
adjustDistortion();//去除運動畸變
calculateSmoothness();//計算曲率，以相鄰左右各五個點計算
markOccludedPoints();//标記瑕點，相鄰但距離較遠的點（平行點被誤判為平面點的情況），以及鄰域距離變化較大的點（遮擋點被誤判為邊點的情況）
extractFeatures();//特征提取
publishCloud(); // cloud for visualization
/**
2. Feature Association
*/
if (!systemInitedLM) {
checkSystemInitialization();//系統初始化，釋出特征點雲用于可視化
return;
}
updateInitialGuess();//先驗位姿
updateTransformation();//更新位姿
integrateTransformation();//變換
publishOdometry();//釋出裡程計資訊
publishCloudsLast(); // cloud to mapOptimization
...
特征提取程式
訂閱處理後的點雲、IMU資料，初始化。
...
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);
subLaserCloudInfo = nh.subscribe<cloud_msgs::cloud_info>("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);
subOutlierCloud = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);
subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this);
...
initializationValue();//變量、容器等初始化指派
...
IMU資料處理
...
//單幀IMU在全局坐标系下的位移、速度、角速度
void AccumulateIMUShiftAndRotation()
{
float roll = imuRoll[imuPointerLast];
float pitch = imuPitch[imuPointerLast];
float yaw = imuYaw[imuPointerLast];
float accX = imuAccX[imuPointerLast];
float accY = imuAccY[imuPointerLast];
float accZ = imuAccZ[imuPointerLast];
//IMU坐标系到世界坐标系的變換
float x1 = cos(roll) * accX - sin(roll) * accY;
float y1 = sin(roll) * accX + cos(roll) * accY;
float z1 = accZ;
float x2 = x1;
float y2 = cos(pitch) * y1 - sin(pitch) * z1;
float z2 = sin(pitch) * y1 + cos(pitch) * z1;
accX = cos(yaw) * x2 + sin(yaw) * z2;
accY = y2;
accZ = -sin(yaw) * x2 + cos(yaw) * z2;
//imu測量周期
int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];
if (timeDiff < scanPeriod) {
imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff + accX * timeDiff * timeDiff / 2;
imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff + accY * timeDiff * timeDiff / 2;
imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff + accZ * timeDiff * timeDiff / 2;
imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;
imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;
imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;
imuAngularRotationX[imuPointerLast] = imuAngularRotationX[imuPointerBack] + imuAngularVeloX[imuPointerBack] * timeDiff;
imuAngularRotationY[imuPointerLast] = imuAngularRotationY[imuPointerBack] + imuAngularVeloY[imuPointerBack] * timeDiff;
imuAngularRotationZ[imuPointerLast] = imuAngularRotationZ[imuPointerBack] + imuAngularVeloZ[imuPointerBack] * timeDiff;
}
}
...
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
//四元數得到旋轉角
double roll, pitch, yaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(imuIn->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
//去除IMU重力加速度影響，同時進行坐标軸變換
float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;
//儲存歐拉角、線加速度、角速度
imuPointerLast = (imuPointerLast + 1) % imuQueLength;
imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
imuRoll[imuPointerLast] = roll;
imuPitch[imuPointerLast] = pitch;
imuYaw[imuPointerLast] = yaw;
imuAccX[imuPointerLast] = accX;
imuAccY[imuPointerLast] = accY;
imuAccZ[imuPointerLast] = accZ;
imuAngularVeloX[imuPointerLast] = imuIn->angular_velocity.x;
imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y;
imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z;
AccumulateIMUShiftAndRotation();//積分得到位移、角度、速度
}
adjustDistortion()
使用IMU資料插值，去除運動畸變
void adjustDistortion()
{
bool halfPassed = false;
int cloudSize = segmentedCloud->points.size();
PointType point;
for (int i = 0; i < cloudSize; i++) {
point.x = segmentedCloud->points[i].y;
point.y = segmentedCloud->points[i].z;
point.z = segmentedCloud->points[i].x;
//調整點所在的位置
float ori = -atan2(point.x, point.z);
if (!halfPassed) {
if (ori < segInfo.startOrientation - M_PI / 2)
ori += 2 * M_PI;
else if (ori > segInfo.startOrientation + M_PI * 3 / 2)
ori -= 2 * M_PI;
if (ori - segInfo.startOrientation > M_PI)
halfPassed = true;
} else {
ori += 2 * M_PI;
if (ori < segInfo.endOrientation - M_PI * 3 / 2)
ori += 2 * M_PI;
else if (ori > segInfo.endOrientation + M_PI / 2)
ori -= 2 * M_PI;
}
//儲存各點的相對時間
float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;
point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;
if (imuPointerLast >= 0) {
float pointTime = relTime * scanPeriod;
imuPointerFront = imuPointerLastIteration;
//與IMU時間戳對齊
while (imuPointerFront != imuPointerLast) {
if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
break;
}
imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}
//imuPointerFront == imuPointerLast，IMU資料比雷射早且無後邊資料，無法插值
if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
imuRollCur = imuRoll[imuPointerFront];
imuPitchCur = imuPitch[imuPointerFront];
imuYawCur = imuYaw[imuPointerFront];
imuVeloXCur = imuVeloX[imuPointerFront];
imuVeloYCur = imuVeloY[imuPointerFront];
imuVeloZCur = imuVeloZ[imuPointerFront];
imuShiftXCur = imuShiftX[imuPointerFront];
imuShiftYCur = imuShiftY[imuPointerFront];
imuShiftZCur = imuShiftZ[imuPointerFront];
} else { //IMU插值，角度、速度、位移
int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
} else {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
}
imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;
imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
}
//此處的更新資料用于updateImuRollPitchYawStartSinCos()
if (i == 0) {
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;
if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];
imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];
imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];
}else{
int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
imuAngularRotationXCur = imuAngularRotationX[imuPointerFront] * ratioFront + imuAngularRotationX[imuPointerBack] * ratioBack;
imuAngularRotationYCur = imuAngularRotationY[imuPointerFront] * ratioFront + imuAngularRotationY[imuPointerBack] * ratioBack;
imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront] * ratioFront + imuAngularRotationZ[imuPointerBack] * ratioBack;
}
imuAngularFromStartX = imuAngularRotationXCur - imuAngularRotationXLast;
imuAngularFromStartY = imuAngularRotationYCur - imuAngularRotationYLast;
imuAngularFromStartZ = imuAngularRotationZCur - imuAngularRotationZLast;
imuAngularRotationXLast = imuAngularRotationXCur;
imuAngularRotationYLast = imuAngularRotationYCur;
imuAngularRotationZLast = imuAngularRotationZCur;
updateImuRollPitchYawStartSinCos();
} else {
VeloToStartIMU();//速度投影到初始時刻
TransformToStartIMU(&point);//點的坐标變換到初始時刻
}
}
segmentedCloud->points[i] = point;
}
imuPointerLastIteration = imuPointerLast;
}
calculateSmoothness()
計算點的曲率，并将特殊情況的點标記為瑕點，不參與後續特征提取。
extractFeatures()
根據曲率門檻值，提取邊特征點、平面特征點，降采樣處理之後釋出。
void extractFeatures()
{
cornerPointsSharp->clear();
cornerPointsLessSharp->clear();
surfPointsFlat->clear();
surfPointsLessFlat->clear();
for (int i = 0; i < N_SCAN; i++) {
surfPointsLessFlatScan->clear();
//将一幀點雲range-image按照行平均分為6份
for (int j = 0; j < 6; j++) {
int sp = (segInfo.startRingIndex[i] * (6 - j) + segInfo.endRingIndex[i] * j) / 6;//start_point_per
int ep = (segInfo.startRingIndex[i] * (5 - j) + segInfo.endRingIndex[i] * (j + 1)) / 6 - 1;//end_point_per
if (sp >= ep)
continue;
std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--) {
int ind = cloudSmoothness[k].ind;
//邊點2個，次邊點18個
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > edgeThreshold &&
segInfo.segmentedCloudGroundFlag[ind] == false) {
largestPickedNum++;
if (largestPickedNum <= 2) {
cloudLabel[ind] = 2;
cornerPointsSharp->push_back(segmentedCloud->points[ind]);
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else if (largestPickedNum <= 20) {
cloudLabel[ind] = 1;
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else {
break;
}
cloudNeighborPicked[ind] = 1;
//特征點前後5個若有距離大于門檻值的可以不用标記，否則标記為非特征點
for (int l = 1; l <= 5; l++) {
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
}
}
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++) {
int ind = cloudSmoothness[k].ind;
//平面點4個
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < surfThreshold &&
segInfo.segmentedCloudGroundFlag[ind] == true) {
cloudLabel[ind] = -1;
surfPointsFlat->push_back(segmentedCloud->points[ind]);
smallestPickedNum++;
if (smallestPickedNum >= 4) {
break;
}
cloudNeighborPicked[ind] = 1;
//
for (int l = 1; l <= 5; l++) {
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
}
}
//未被标記的點以及上述平面點作為次平面點
for (int k = sp; k <= ep; k++) {
if (cloudLabel[k] <= 0) {
surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);
}
}
}
surfPointsLessFlatScanDS->clear();
//平面點降采樣
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.filter(*surfPointsLessFlatScanDS);
*surfPointsLessFlat += *surfPointsLessFlatScanDS;
}
}
特征關聯程式
将目前時刻儲存的IMU資料作為先驗資訊（角度、位移）
updateTransformation()
查找平面特征進行比對，尋找最近鄰三點構成平面，建構點面距離最小二乘求解位姿（高斯牛頓）；查找線特征進行比對，尋找兩點構成直線，建構點線距離最小二乘求解位姿（高斯牛頓）。
注：預設Lidar坐标系是右下前
void updateTransformation(){
if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
return;
for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
laserCloudOri->clear();
coeffSel->clear();
findCorrespondingSurfFeatures(iterCount1);//找對應的特征平面，計算協方差矩陣
if (laserCloudOri->points.size() < 10)
continue;
//通過面特征比對計算變換矩陣(z,roll,pitch)
if (calculateTransformationSurf(iterCount1) == false)
break;
}
for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {
laserCloudOri->clear();
coeffSel->clear();
findCorrespondingCornerFeatures(iterCount2);//找對應的特征線，計算協方差矩陣
if (laserCloudOri->points.size() < 10)
continue;
//通過線特征比對結合面特征得出的變換矩陣計算變換矩陣(x,y,yaw)
if (calculateTransformationCorner(iterCount2) == false)
break;
}
}
integrateTransformation()
将前後兩幀位姿累加，獲得對第一幀的位姿，同時從局部坐标系轉換到全局坐标系，加入IMU資料更新出位姿，釋出裡程計資訊、tf變換、點雲。
void integrateTransformation(){
float rx, ry, rz, tx, ty, tz;
AccumulateRotation(transformSum[0], transformSum[1], transformSum[2],
-transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);//将兩幀之間的位姿累加，獲得相對于第一幀的旋轉矩陣，局部坐标系轉化為全局坐标系
//更新平移量
float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX)
- sin(rz) * (transformCur[4] - imuShiftFromStartY);
float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX)
+ cos(rz) * (transformCur[4] - imuShiftFromStartY);
float z1 = transformCur[5] - imuShiftFromStartZ;
float x2 = x1;
float y2 = cos(rx) * y1 - sin(rx) * z1;
float z2 = sin(rx) * y1 + cos(rx) * z1;
tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
ty = transformSum[4] - y2;
tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart,
imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);//加入IMU資料更新位姿
transformSum[0] = rx;
transformSum[1] = ry;
transformSum[2] = rz;
transformSum[3] = tx;
transformSum[4] = ty;
transformSum[5] = tz;
}
參考資料：
[1]. lego-loam閱讀了解筆記
[2]. loam代碼解析
[3]. 雷射雷達slam之LOAM中的坐标轉換與IMU融合
[4]. 三維SLAM算法LeGO-LOAM源碼閱讀
[5]. LEGO-LOAM源碼解析 — FeatureAssociation節點(3)
[6]. LeGo-LOAM源碼閱讀筆記(三)—featureAssociation.cpp
[7]. LeGO-LOAM中的數學公式推導
![查找算法之二分查找查找算法之二分查找[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)