featureAssociation.cpp
其次featureAssociation
这一个node节点,主要是特征提取。代码中先初始化了lego_loam::FeatureAssociation
,用来订阅了上一节点发出来的分割出来的点云,点云的属性,外点以及IMU消息,并设置了回调函数。其中IMU消息的订阅函数较为复杂,它从IMU数据中提取出姿态,角速度和线加速度,其中姿态用来消除重力对线加速度的影响。然后函数FeatureAssociation::AccumulateIMUShiftAndRotation
用来做积分,包括根据姿态,将加速度往世界坐标系下进行投影。再根据匀加速度运动模型积分得到速度和位移,同时,对角速度也进行了积分。
void FeatureAssociation::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);
// 对加速度进行坐标变换
// 进行加速度坐标交换时将重力加速度去除,然后再进行xxx到zzz,yyy到xxx,zzz到yyy的变换。
// 去除重力加速度的影响时,需要把重力加速度分解到三个坐标轴上,然后分别去除他们分量的影响,在去除的过程中需要注意加减号(默认右手坐标系的旋转方向来看)。
// 在上面示意图中,可以简单理解为红色箭头实线分解到红色箭头虚线上(根据pitchpitchpitch进行分解),然后再按找rollrollroll角进行分解。
// 原文链接:https://blog.csdn.net/wykxwyc/article/details/98317544
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();
}
上面的回调函数仅仅是考虑该如何读取数据,而未涉及点云该如何处理,也没有说怎么与IMU数据做融合。由于代码太多而且很多都是纯粹的数学推导计算,为此我们选取runFeatureAssociation
中的一些重要的思想来详写。
// 主程序入口
void FeatureAssociation::runFeatureAssociation() {
// 有新数据进来才执行
if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05) {
newSegmentedCloud = false;
newSegmentedCloudInfo = false;
newOutlierCloud = false;
} else {
return;
}
/**
1. Feature Extraction
*/
adjustDistortion(); // imu去畸变
calculateSmoothness(); // 计算光滑性
markOccludedPoints(); // 距离较大或者距离变动较大的点标记
extractFeatures(); // 特征提取(未看懂)
publishCloud(); // cloud for visualization
/**
2. Feature Association
*/
if (!systemInitedLM) {
checkSystemInitialization();
return;
}
updateInitialGuess(); // 更新初始位姿
// 一个是找特征平面,通过面之间的对应关系计算出变换矩阵。
// 另一个部分是通过角、边特征的匹配,计算变换矩阵。
updateTransformation();
integrateTransformation(); // 计算旋转角的累积变化量。
publishOdometry();
publishCloudsLast(); // cloud to mapOptimization
}
- adjustDistortion:畸变校正。前面IMU消息的回调函数中对数据进行了转存和积分,这里是根据激光雷达一帧扫描期间,IMU积分得到的位姿变换,通过插值的形式,对点云畸变进行校正。由于激光雷达与IMU坐标系定义不同,因此函数中先对坐标进行了交换,仅仅是为了将点云在IMU坐标系下表示。
下面引用下网络上对该畸变校正步骤的举例:对于某个激光`pi`,采集到该点时雷达本身的位姿`Ti`已经与采集第一个点`p0`时的位姿`T0`不同了,如果我们还认为这个点也是在`T0`处获取到的,这是不准确的,特别是当雷达扫描频率比较低,且雷达运动比较剧烈的情况下,所以,**需要根据`T0`和`Ti`之间的相对变换,对点`pi`进行补偿**,补偿的结果,就是这个`Ti`位姿处观测到的`pi`,如果是在`T0`处被观测到的,它的坐标应该是怎样的,这样就消除了雷达本身运动周期的影响,经过畸变消除,可以认为,所有点都是一瞬间在`T0`处采集得到的,而不存在内畸变的问题。然后,**问题就变成了如何确定`Ti`与`T0`之间的相对位姿**,我们知道IMU一直在计算积分,频率远远比雷达扫描要高,但是两者的时间戳是离散的并不是一一对应的,因此,采集到`pi`点的时刻`ti`时雷达的位姿并**不能直接给出来**,但是,IMU可以给出前后两个距离`ti`**非常接近的时刻积分出来的位姿**,这个由于IMU频率非常高所以两者之间时间非常短,可以**通过线性插值的形式确定`ti`时刻的位姿`Ti`**。
```cpp
void FeatureAssociation::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;
// 计算点的yaw, 根据不同的偏航角,可以知道激光雷达扫过的位置有没有超过一半
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;
// imu与lidar时间轴对齐
if (imuPointerLast >= 0) {
float pointTime = relTime * scanPeriod;
imuPointerFront = imuPointerLastIteration;
// imu数据比激光数据早,但是没有更后面的数据,不能通过插补进行优化
while (imuPointerFront != imuPointerLast) {
if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
break;
}
imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}
// 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数据充足的情况下才会发生插值
// 当前timeScanCur + pointTime < imuTime[imuPointerFront],
// 而且imuPointerFront是最早一个时间大于timeScanCur + pointTime的imu数据指针。
// imuPointerBack是imuPointerFront的前一个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]);
// roll和pitch通常接近0,yaw变化角度较大
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;
}
// i = 0 时刻
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:根据两侧的10个点,计算曲率,这里的曲率只是一个量的大小的概念。因为曲率并不参加最终的优化,只是衡量一个点光滑与否的标志,是个相对的概念。所以只是将周围十个点累加,并未开方求欧氏距离。最后在
cloudSmoothness
这个vector中保存了曲率与点索引对,以便后面根据曲率对点进行排序,并根据索引对直接得到点的索引顺序。void FeatureAssociation::calculateSmoothness() { // 计算光滑性 int cloudSize = segmentedCloud->points.size(); for (int i = 5; i < cloudSize - 5; i++) { float diffRange = segInfo.segmentedCloudRange[i - 5] + segInfo.segmentedCloudRange[i - 4] + segInfo.segmentedCloudRange[i - 3] + segInfo.segmentedCloudRange[i - 2] + segInfo.segmentedCloudRange[i - 1] - segInfo.segmentedCloudRange[i] * 10 + segInfo.segmentedCloudRange[i + 1] + segInfo.segmentedCloudRange[i + 2] + segInfo.segmentedCloudRange[i + 3] + segInfo.segmentedCloudRange[i + 4] + segInfo.segmentedCloudRange[i + 5]; cloudCurvature[i] = diffRange * diffRange; // 在markOccludedPoints()函数中对该参数进行重新修改 cloudNeighborPicked[i] = 0; cloudLabel[i] = 0; // 在extractFeatures()函数中会对标签进行修改 cloudSmoothness[i].value = cloudCurvature[i]; cloudSmoothness[i].ind = i; } }
-
markOccludedPoints:对于深度变化比较明显的相邻两个点,远处的那一个更容易被遮挡,不稳定;如果某个点,和两侧的点距离差距都比较大,那么有可能是噪声等,也不稳定。在
cloudNeighborPicked
中标注为1,后面的特征提取中不会再考虑。void FeatureAssociation::markOccludedPoints() { int cloudSize = segmentedCloud->points.size(); for (int i = 5; i < cloudSize - 6; ++i) { float depth1 = segInfo.segmentedCloudRange[i]; float depth2 = segInfo.segmentedCloudRange[i + 1]; int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i + 1] - segInfo.segmentedCloudColInd[i])); if (columnDiff < 10) { // 选择距离较远的那些点,并将他们标记为1 if (depth1 - depth2 > 0.3) { cloudNeighborPicked[i - 5] = 1; cloudNeighborPicked[i - 4] = 1; cloudNeighborPicked[i - 3] = 1; cloudNeighborPicked[i - 2] = 1; cloudNeighborPicked[i - 1] = 1; cloudNeighborPicked[i] = 1; } else if (depth2 - depth1 > 0.3) { cloudNeighborPicked[i + 1] = 1; cloudNeighborPicked[i + 2] = 1; cloudNeighborPicked[i + 3] = 1; cloudNeighborPicked[i + 4] = 1; cloudNeighborPicked[i + 5] = 1; cloudNeighborPicked[i + 6] = 1; } } float diff1 = std::abs(float(segInfo.segmentedCloudRange[i - 1] - segInfo.segmentedCloudRange[i])); float diff2 = std::abs(float(segInfo.segmentedCloudRange[i + 1] - segInfo.segmentedCloudRange[i])); // 选择距离变化较大的点,并将他们标记为1 if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && diff2 > 0.02 * segInfo.segmentedCloudRange[i]) cloudNeighborPicked[i] = 1; } }
-
extractFeatures:则是根据曲率的大小,提取四种特征点。这里的流程和LOAM基本一样,将点分为多种类型,把每个线分成了6个区,在每个区内提取一定数量的角点和面点,这是为了让点分布更均匀,匹配时效果会更稳定更好。然后就是在一个区的范围内,按照曲率对索引进行排序,曲率最大的20个非地面点为角点,其中最最大的2个强角点,两者是包含的关系;曲率最最小的4个地面点为强面点,其余所有地面点都是一般面点,点数太多需要进行降采样,一般是五个点取一次。在确定提取某个点后,其周围一定范围内的点
cloudNeighborPicked
会被标注为1,认为该区域的点已经被选择,不会再次被选中,这是为了防止提取出来的特征点过度聚集在某一处。void FeatureAssociation::extractFeatures() { cornerPointsSharp->clear(); cornerPointsLessSharp->clear(); surfPointsFlat->clear(); surfPointsLessFlat->clear(); // 提取4种特征点 for (int i = 0; i < N_SCAN; i++) { surfPointsLessFlatScan->clear(); for (int j = 0; j < 6; j++) { int sp = (segInfo.startRingIndex[i] * (6 - j) + segInfo.endRingIndex[i] * j) / 6; int ep = (segInfo.startRingIndex[i] * (5 - j) + segInfo.endRingIndex[i] * (j + 1)) / 6 - 1; 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; 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; 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; 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]); } } } // surfPointsLessFlatScan中有过多的点云,如果点云太多,计算量太大 surfPointsLessFlatScanDS->clear(); downSizeFilter.setInputCloud(surfPointsLessFlatScan); downSizeFilter.filter(*surfPointsLessFlatScanDS); *surfPointsLessFlat += *surfPointsLessFlatScanDS; } }
-
publishCloud:发布4种特征点,分别是
cornerPointsSharp
、cornerPointsLessSharp
、surfPointsFlat
、surfPointsLessFlat
。用来记录在一帧激光雷达数据下提取的特征点。void FeatureAssociation::publishCloud() { sensor_msgs::PointCloud2 laserCloudOutMsg; if (pubCornerPointsSharp.getNumSubscribers() != 0) { pcl::toROSMsg(*cornerPointsSharp, laserCloudOutMsg); laserCloudOutMsg.header.stamp = cloudHeader.stamp; laserCloudOutMsg.header.frame_id = "/camera"; pubCornerPointsSharp.publish(laserCloudOutMsg); } if (pubCornerPointsLessSharp.getNumSubscribers() != 0) { pcl::toROSMsg(*cornerPointsLessSharp, laserCloudOutMsg); laserCloudOutMsg.header.stamp = cloudHeader.stamp; laserCloudOutMsg.header.frame_id = "/camera"; pubCornerPointsLessSharp.publish(laserCloudOutMsg); } if (pubSurfPointsFlat.getNumSubscribers() != 0) { pcl::toROSMsg(*surfPointsFlat, laserCloudOutMsg); laserCloudOutMsg.header.stamp = cloudHeader.stamp; laserCloudOutMsg.header.frame_id = "/camera"; pubSurfPointsFlat.publish(laserCloudOutMsg); } if (pubSurfPointsLessFlat.getNumSubscribers() != 0) { pcl::toROSMsg(*surfPointsLessFlat, laserCloudOutMsg); laserCloudOutMsg.header.stamp = cloudHeader.stamp; laserCloudOutMsg.header.frame_id = "/camera"; pubSurfPointsLessFlat.publish(laserCloudOutMsg); } }
此时根据点云与点云之间曲率来提取符合的特征点信息,下面则是根据特征点的匹配来计算出面与面的变换矩阵。
-
checkSystemInitialization:该函数主要执行的是激光里程计初始化。激光里程计是连续帧之间匹配的过程,需要有一个起始参考帧,如果系统没有经过初始化,那么就把当前帧作为参考帧,并初始化当前总的位姿
transformSum
(除了俯仰角和滚转角外,其余参数全部初始化为0)。void FeatureAssociation::checkSystemInitialization() { // 交换两团点云,并发布出去 pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp; cornerPointsLessSharp = laserCloudCornerLast; laserCloudCornerLast = laserCloudTemp; laserCloudTemp = surfPointsLessFlat; surfPointsLessFlat = laserCloudSurfLast; laserCloudSurfLast = laserCloudTemp; kdtreeCornerLast->setInputCloud(laserCloudCornerLast); kdtreeSurfLast->setInputCloud(laserCloudSurfLast); laserCloudCornerLastNum = laserCloudCornerLast->points.size(); laserCloudSurfLastNum = laserCloudSurfLast->points.size(); sensor_msgs::PointCloud2 laserCloudCornerLast2; pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); laserCloudCornerLast2.header.stamp = cloudHeader.stamp; laserCloudCornerLast2.header.frame_id = "/camera"; pubLaserCloudCornerLast.publish(laserCloudCornerLast2); sensor_msgs::PointCloud2 laserCloudSurfLast2; pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); laserCloudSurfLast2.header.stamp = cloudHeader.stamp; laserCloudSurfLast2.header.frame_id = "/camera"; pubLaserCloudSurfLast.publish(laserCloudSurfLast2); transformSum[0] += imuPitchStart; transformSum[2] += imuRollStart; systemInitedLM = true; }
-
updateInitialGuess:该函数根据IMU积分的结果,计算出一个初始位姿transformCur,这个位姿指的是雷达旋转一圈后发生的相对位姿变换。
void FeatureAssociation::updateInitialGuess() { imuPitchLast = imuPitchCur; imuYawLast = imuYawCur; imuRollLast = imuRollCur; imuShiftFromStartX = imuShiftFromStartXCur; imuShiftFromStartY = imuShiftFromStartYCur; imuShiftFromStartZ = imuShiftFromStartZCur; imuVeloFromStartX = imuVeloFromStartXCur; imuVeloFromStartY = imuVeloFromStartYCur; imuVeloFromStartZ = imuVeloFromStartZCur; // imu是在start坐标系下 if (imuAngularFromStartX != 0 || imuAngularFromStartY != 0 || imuAngularFromStartZ != 0) { transformCur[0] = -imuAngularFromStartY; transformCur[1] = -imuAngularFromStartZ; transformCur[2] = -imuAngularFromStartX; } // 速度×时间 if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0) { transformCur[3] -= imuVeloFromStartX * scanPeriod; transformCur[4] -= imuVeloFromStartY * scanPeriod; transformCur[5] -= imuVeloFromStartZ * scanPeriod; } }
-
updateTransformation:通过约束优化相对位姿。对于当前帧的每一个面点,在上一帧找到最近邻以及另外两个不共线的点组成面,该点到面的距离就是要减小的残差,通过面之间的对应关系计算出变换矩阵。之后通过角、边特征的匹配,计算变换矩阵。即找到最近邻和另外一个点构建约束,被优化的对象就是transformCur,即找到一个相对位姿变换,使得总体的残差最小。然后依次使用平面点和角点,对相对位姿进行优化。优化的对象
transformCur
特点在于,先使用平面点经历多次优化后,再使用角点(可能因为平面点更多,因而会更稳定。void FeatureAssociation::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; // 面特征匹配计算变换矩阵 if (calculateTransformationSurf(iterCount1) == false) break; } for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) { laserCloudOri->clear(); coeffSel->clear(); // 寻找边、角特征 findCorrespondingCornerFeatures(iterCount2); if (laserCloudOri->points.size() < 10) continue; // 边角特征匹配 if (calculateTransformationCorner(iterCount2) == false) break; } }
-
integrateTransformation:把优化得到的transformCur,累加到transformSum中。
void FeatureAssociation::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); // 与accumulateRotatio联合起来更新transformSum的rotation部分的工作 // 可视为transformToEnd的下部分的逆过程 PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz); transformSum[0] = rx; transformSum[1] = ry; transformSum[2] = rz; transformSum[3] = tx; transformSum[4] = ty; transformSum[5] = tz; }
- publishOdometry:发布里程计位姿位姿和tf变换,这里的变换实际上是odo相对于地图原点的,存在持续的误差积累的;
- publishCloudsLast:根据优化的结果transformCur,将当前帧特征点云统一到终止时刻,用来作为下次匹配的参考(因为上一帧的点云已经被统一到当前帧的起始时刻,匹配是当前帧的终止位姿相对于起始位姿的转化),同时发布特征点云。
mapOptmization.cpp
最后我们对mapOptmization
这一个node进行解析,前两部分已经完成了一个激光雷达里程计该做的处理(点云预处理,连续帧匹配计算出激光里程计信息)。但是这个过程中误差是逐渐累积的,为此我们需要通过回环检测来减小误差,在地图优化部分,相比于LeGO-LOAM而言,文中使用ScanContext来实现scan-to-map的匹配,并检测到回环以及进行优化。
与前面两个node类似,mapOptmization
的主要函数均封装在run
函数中,同时作者在网上找到了一个较为清晰的流程图,因此下面会挑选重点函数进行详细阐述。
void mapOptimization::run() {
// 有新数据进来,才执行后续
if (newLaserCloudCornerLast && std::abs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
newLaserCloudSurfLast && std::abs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
newLaserCloudOutlierLast && std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&
newLaserOdometry) {
newLaserCloudCornerLast = false;
newLaserCloudSurfLast = false;
newLaserCloudOutlierLast = false;
newLaserOdometry = false;
std::lock_guard<std::mutex> lock(mtx);
if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
timeLastProcessing = timeLaserOdometry;
transformAssociateToMap(); // 坐标系->map
extractSurroundingKeyFrames();
downsampleCurrentScan();
scan2MapOptimization();
saveKeyFramesAndFactor();
correctPoses();
publishTF();
publishKeyPosesAndFrames();
clearCloud();
}
}
}
前面的transformAssociateToMap
、extractSurroundingKeyFrames
以及downsampleCurrentScan
函数主要是实现了坐标系的转换、时间顺序,选择最近的一定数量的关键帧点云,组成局部地图laserCloudCornerFromMap
、以及当前帧原始点云,角点,面点,离群点进行降采样。主要是对之前的数据进行降采样以进行快速匹配。
-
scan2MapOptimization和saveKeyFramesAndFactor:这两个函数主要是用来处理因子图优化的功能。
scan2MapOptimization
内部主要是使用scan-to-model
位姿优化,获得当前时间点机器人的位姿transformTobeMapped
,使得总体残差最小,连续循环优化多次。此外该部分的优化会参考IMU消息回调所确定的roll和pitch对该位姿进行修正,对transformTobeMapped
进行中值滤波,获得最终的机器人位姿,到这里,虽然在scan-to-scan之后,又进行了scan-to-map的匹配,但是并未出现回环检测和优化,所以依然是一个误差不断积累的里程计的概念void mapOptimization::scan2MapOptimization() { // laserCloudCornerFromMapDSNum是extractSurroundingKeyFrames()函数最后降采样得到的coner点云数 // laserCloudSurfFromMapDSNum是extractSurroundingKeyFrames()函数降采样得到的surface点云数 if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) { // laserCloudCornerFromMapDS和laserCloudSurfFromMapDS的来源有2个: // 当有闭环时,来源是:recentCornerCloudKeyFrames,没有闭环时,来源surroundingCornerCloudKeyFrames kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS); kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS); for (int iterCount = 0; iterCount < 10; iterCount++) { laserCloudOri->clear(); coeffSel->clear(); cornerOptimization(iterCount); surfOptimization(iterCount); if (LMOptimization(iterCount) == true) break; } // 迭代结束更新相关的转移矩阵 transformUpdate(); } }
saveKeyFramesAndFactor
主要作用是选定关键帧。如果距离上一次保存的关键帧欧式距离最够大,需要保存当前关键帧,则需要我们计算与上一关键帧之间的约束,这种约束可以理解为局部的小回环,加入后端进行优化。将优化的结果保存作为关键帧位姿(保存当前关键帧的3维和6维位姿)和点云,同步到scan-to-map优化环节(即修改transformAftMapped
【transformAssociateToMap函数中调用】和transformTobeMapped
【scan2MapOptimization函数中调用】)。为了检测全局的大回环,还需要生成当前关键帧的ScanContext,即scManager.makeAndSaveScancontextAndKeys
中。void mapOptimization::saveKeyFramesAndFactor() { // 此函数保存关键帧和factor currentRobotPosPoint.x = transformAftMapped[3]; currentRobotPosPoint.y = transformAftMapped[4]; currentRobotPosPoint.z = transformAftMapped[5]; bool saveThisKeyFrame = true; if (sqrt((previousRobotPosPoint.x - currentRobotPosPoint.x) * (previousRobotPosPoint.x - currentRobotPosPoint.x) + (previousRobotPosPoint.y - currentRobotPosPoint.y) * (previousRobotPosPoint.y - currentRobotPosPoint.y) + (previousRobotPosPoint.z - currentRobotPosPoint.z) * (previousRobotPosPoint.z - currentRobotPosPoint.z)) < 0.3) { // save keyframe every 0.3 meter saveThisKeyFrame = false; } if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty()) return; previousRobotPosPoint = currentRobotPosPoint; /** * update grsam graph */ if (cloudKeyPoses3D->points.empty()) { gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]), Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise)); initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]), Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4]))); for (int i = 0; i < 6; ++i) transformLast[i] = transformTobeMapped[i]; } else { gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]), Point3(transformLast[5], transformLast[3], transformLast[4])); gtsam::Pose3 poseTo = Pose3( Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]), Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4])); gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->points.size() - 1, cloudKeyPoses3D->points.size(), poseFrom.between(poseTo), odometryNoise)); initialEstimate.insert(cloudKeyPoses3D->points.size(), Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]), Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]))); } /** * update iSAM */ isam->update(gtSAMgraph, initialEstimate); isam->update(); gtSAMgraph.resize(0); initialEstimate.clear(); /** * save key poses */ PointType thisPose3D; PointTypePose thisPose6D; Pose3 latestEstimate; isamCurrentEstimate = isam->calculateEstimate(); latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size() - 1); // cloudKeyPoses3D指当前的点云,cloudKeyPoses6D指带pose的 thisPose3D.x = latestEstimate.translation().y(); thisPose3D.y = latestEstimate.translation().z(); thisPose3D.z = latestEstimate.translation().x(); thisPose3D.intensity = cloudKeyPoses3D->points.size(); // this can be used as index cloudKeyPoses3D->push_back(thisPose3D); thisPose6D.x = thisPose3D.x; thisPose6D.y = thisPose3D.y; thisPose6D.z = thisPose3D.z; thisPose6D.intensity = thisPose3D.intensity; // this can be used as index thisPose6D.roll = latestEstimate.rotation().pitch(); thisPose6D.pitch = latestEstimate.rotation().yaw(); thisPose6D.yaw = latestEstimate.rotation().roll(); // in camera frame thisPose6D.time = timeLaserOdometry; cloudKeyPoses6D->push_back(thisPose6D); /** * save updated transform */ if (cloudKeyPoses3D->points.size() > 1) { transformAftMapped[0] = latestEstimate.rotation().pitch(); transformAftMapped[1] = latestEstimate.rotation().yaw(); transformAftMapped[2] = latestEstimate.rotation().roll(); transformAftMapped[3] = latestEstimate.translation().y(); transformAftMapped[4] = latestEstimate.translation().z(); transformAftMapped[5] = latestEstimate.translation().x(); for (int i = 0; i < 6; ++i) { transformLast[i] = transformAftMapped[i]; transformTobeMapped[i] = transformAftMapped[i]; } } pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>()); pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>()); pcl::PointCloud<PointType>::Ptr thisOutlierKeyFrame(new pcl::PointCloud<PointType>()); pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame); pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame); pcl::copyPointCloud(*laserCloudOutlierLastDS, *thisOutlierKeyFrame); /* Scan Context loop detector - ver 1: using surface feature as an input point cloud for scan context (2020.04.01: checked it works.) - ver 2: using downsampled original point cloud (/full_cloud_projected + downsampling) */ bool usingRawCloud = true; if (usingRawCloud) { // v2 uses downsampled raw point cloud, more fruitful height information than using feature points (v1) // 这里对点云提取scan context特征 pcl::PointCloud<PointType>::Ptr thisRawCloudKeyFrame(new pcl::PointCloud<PointType>()); pcl::copyPointCloud(*laserCloudRawDS, *thisRawCloudKeyFrame); scManager.makeAndSaveScancontextAndKeys(*thisRawCloudKeyFrame); } else { // v1 uses thisSurfKeyFrame, it also works. (empirically checked at Mulran dataset sequences) scManager.makeAndSaveScancontextAndKeys(*thisSurfKeyFrame); } cornerCloudKeyFrames.push_back(thisCornerKeyFrame); surfCloudKeyFrames.push_back(thisSurfKeyFrame); outlierCloudKeyFrames.push_back(thisOutlierKeyFrame); } // saveKeyFramesAndFactor
-
correctPoses:如果ScanContext回环检测(即std::thread loopthread)对全局位姿进行了优化,需要同步下来优化后的关键帧所在的位姿。判断是否回环在
performLoopClosure
函数中。void mapOptimization::performLoopClosure(void) { if (cloudKeyPoses3D->points.empty() == true) return; // try to find close key frame if there are any if (potentialLoopFlag == false) { if (detectLoopClosure() == true) { std::cout << std::endl; potentialLoopFlag = true; // find some key frames that is old enough or close enough for loop closure timeSaveFirstCurrentScanForLoopClosure = timeLaserOdometry; } if (potentialLoopFlag == false) { return; } } // reset the flag first no matter icp successes or not potentialLoopFlag = false; // ***** // Main // ***** // make common variables at forward float x, y, z, roll, pitch, yaw; Eigen::Affine3f correctionCameraFrame; float noiseScore = 0.5; // constant is ok... gtsam::Vector Vector6(6); Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore; constraintNoise = noiseModel::Diagonal::Variances(Vector6); robustNoiseModel = gtsam::noiseModel::Robust::Create( gtsam::noiseModel::mEstimator::Cauchy::Create(1), // optional: replacing Cauchy by DCS or GemanMcClure gtsam::noiseModel::Diagonal::Variances(Vector6) ); // - checked it works. but with robust kernel, map modification may be delayed (i.e,. requires more true-positive loop factors) bool isValidRSloopFactor = false; bool isValidSCloopFactor = false; /* * 1. RS loop factor (radius search) * 将RS检测到的历史帧和当前帧匹配,求transform, 作为约束边 */ if (RSclosestHistoryFrameID != -1) { pcl::IterativeClosestPoint<PointType, PointType> icp; icp.setMaxCorrespondenceDistance(100); icp.setMaximumIterations(100); icp.setTransformationEpsilon(1e-6); icp.setEuclideanFitnessEpsilon(1e-6); icp.setRANSACIterations(0); // Align clouds icp.setInputSource(RSlatestSurfKeyFrameCloud); icp.setInputTarget(RSnearHistorySurfKeyFrameCloudDS); pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>()); icp.align(*unused_result); // 通过score阈值判定icp是否匹配成功 std::cout << "[RS] ICP fit score: " << icp.getFitnessScore() << std::endl; if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore) { std::cout << "[RS] Reject this loop (bad icp fit score, > " << historyKeyframeFitnessScore << ")" << std::endl; isValidRSloopFactor = false; } else { std::cout << "[RS] The detected loop factor is added between Current [ " << latestFrameIDLoopCloure << " ] and RS nearest [ " << RSclosestHistoryFrameID << " ]" << std::endl; isValidRSloopFactor = true; } // 这里RS检测成功,加入约束边 if (isValidRSloopFactor == true) { correctionCameraFrame = icp.getFinalTransformation(); // get transformation in camera frame (because points are in camera frame) pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw); Eigen::Affine3f correctionLidarFrame = pcl::getTransformation(z, x, y, yaw, roll, pitch); // transform from world origin to wrong pose Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar( cloudKeyPoses6D->points[latestFrameIDLoopCloure]); // transform from world origin to corrected pose Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong; // pre-multiplying -> successive rotation about a fixed frame pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw); gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)); gtsam::Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[RSclosestHistoryFrameID]); gtsam::Vector Vector6(6); std::lock_guard<std::mutex> lock(mtx); gtSAMgraph.add( BetweenFactor<Pose3>(latestFrameIDLoopCloure, RSclosestHistoryFrameID, poseFrom.between(poseTo), robustNoiseModel)); isam->update(gtSAMgraph); isam->update(); gtSAMgraph.resize(0); } } /* * 2. SC loop factor (scan context) * SC检测成功,进行icp匹配 */ if (SCclosestHistoryFrameID != -1) { pcl::IterativeClosestPoint<PointType, PointType> icp; icp.setMaxCorrespondenceDistance(100); icp.setMaximumIterations(100); icp.setTransformationEpsilon(1e-6); icp.setEuclideanFitnessEpsilon(1e-6); icp.setRANSACIterations(0); // Align clouds // Eigen::Affine3f icpInitialMatFoo = pcl::getTransformation(0, 0, 0, yawDiffRad, 0, 0); // because within cam coord: (z, x, y, yaw, roll, pitch) // Eigen::Matrix4f icpInitialMat = icpInitialMatFoo.matrix(); icp.setInputSource(SClatestSurfKeyFrameCloud); icp.setInputTarget(SCnearHistorySurfKeyFrameCloudDS); pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>()); icp.align(*unused_result); // icp.align(*unused_result, icpInitialMat); // PCL icp non-eye initial is bad ... don't use (LeGO LOAM author also said pcl transform is weird.) std::cout << "[SC] ICP fit score: " << icp.getFitnessScore() << std::endl; if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore) { std::cout << "[SC] Reject this loop (bad icp fit score, > " << historyKeyframeFitnessScore << ")" << std::endl; isValidSCloopFactor = false; } else { std::cout << "[SC] The detected loop factor is added between Current [ " << latestFrameIDLoopCloure << " ] and SC nearest [ " << SCclosestHistoryFrameID << " ]" << std::endl; isValidSCloopFactor = true; } // icp匹配成功也加入约束边 if (isValidSCloopFactor == true) { correctionCameraFrame = icp.getFinalTransformation(); // get transformation in camera frame (because points are in camera frame) pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw); gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)); gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); std::lock_guard<std::mutex> lock(mtx); // gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise)); // original gtSAMgraph.add( BetweenFactor<Pose3>(latestFrameIDLoopCloure, SCclosestHistoryFrameID, poseFrom.between(poseTo), robustNoiseModel)); // giseop isam->update(gtSAMgraph); isam->update(); gtSAMgraph.resize(0); } } // flagging aLoopIsClosed = true; } // performLoopClosure
-
publishTF:发布优化后的位姿和tf变换。
- publishKeyPosesAndFrames:发布所有关键帧位姿,当前的局部面点地图及当前帧中的面点/角点。
到这里对SC-LeGO-LOAM的所有流程已经理清楚,同时这部分代码可以在我个人的Github项目中看到,如果各位感兴趣的可以下载测试学习。
评论(0)
您还未登录,请登录后发表或查看评论