LeGo-LOAM激光雷达定位算法源码阅读(三)
文章目录
- 1. mapOptmization框架
- 1.1节点代码主体
- 2.mapOptimization构造函数
- 3. loopClosureThread闭环检测
- 3.1 loopClosureThread
- 3.2 performLoopClosure
- 4.visualizeGlobalMapThread可视化
- 4.1visualizeMapThread
- 5.核心run函数
- 5.1transformAssociateToMap
- 5.2extractSurroundingKeyFrames
- 5.3downsampleCurrentScan
- 5.4scan2MapOptimization
- 5.5saveKeyFramesAndFactor
- 5.6correctPoses
无人驾驶算法学习（九）：LeGo-LOAM激光雷达定位算法
LeGo-LOAM激光雷达定位算法源码阅读(一)
LeGo-LOAM激光雷达定位算法源码阅读(二)
1. mapOptmization框架
1.1节点代码主体
int main(int argc, char** argv)
{
ros::init(argc, argv, "lego_loam");
ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");
mapOptimization MO;//调用构造函数,订阅话题上一帧的surf,corner,outlier点云,imu,和laser_odom,
// std::thread 构造函数，将MO作为参数传入构造的线程中使用
// 进行闭环检测与闭环的功能
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
// 该线程中进行的工作是publishGlobalMap(),将数据发布到ros中，可视化
std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
ros::Rate rate(200);
while (ros::ok())//200hz循环调用
{
ros::spinOnce();
MO.run();//最重要的run()函数
rate.sleep();
}
loopthread.join();
visualizeMapThread.join();
return 0;
}
2.mapOptimization构造函数
mapOptimization():
nh("~")
{
// 用于闭环图优化的参数设置，使用gtsam库
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
isam = new ISAM2(parameters);//初始化了ISAM2对象
pubKeyPoses = nh.advertise("/key_pose_origin", 2);
pubLaserCloudSurround = nh.advertise("/laser_cloud_surround", 2);
pubOdomAftMapped = nh.advertise ("/aft_mapped_to_init", 5);//发布优化后的pose
subLaserCloudCornerLast = nh.subscribe("/laser_cloud_corner_last", 2, &mapOptimization::laserCloudCornerLastHandler, this);
subLaserCloudSurfLast = nh.subscribe("/laser_cloud_surf_last", 2, &mapOptimization::laserCloudSurfLastHandler, this);
subOutlierCloudLast = nh.subscribe("/outlier_cloud_last", 2, &mapOptimization::laserCloudOutlierLastHandler, this);
subLaserOdometry = nh.subscribe("/laser_odom_to_init", 5, &mapOptimization::laserOdometryHandler, this);//订阅话题
subImu = nh.subscribe (imuTopic, 50, &mapOptimization::imuHandler, this);
pubHistoryKeyFrames = nh.advertise("/history_cloud", 2);
pubIcpKeyFrames = nh.advertise("/corrected_cloud", 2);
pubRecentKeyFrames = nh.advertise("/recent_cloud", 2);//发布的点云话题
// 下采样参数
//设置滤波时创建的体素大小为0.2m/0.4m立方体,下面的单位为m
downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);
downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);
downSizeFilterOutlier.setLeafSize(0.4, 0.4, 0.4);
downSizeFilterHistoryKeyFrames.setLeafSize(0.4, 0.4, 0.4);
downSizeFilterSurroundingKeyPoses.setLeafSize(1.0, 1.0, 1.0);
downSizeFilterGlobalMapKeyPoses.setLeafSize(1.0, 1.0, 1.0);
downSizeFilterGlobalMapKeyFrames.setLeafSize(0.4, 0.4, 0.4);
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
aftMappedTrans.frame_id_ = "/camera_init";
aftMappedTrans.child_frame_id_ = "/aft_mapped";
allocateMemory();//分配了内存
}
3. loopClosureThread闭环检测
3.1 loopClosureThread
void loopClosureThread(){
//它的主要功能是进行闭环检测和闭环修正
if (loopClosureEnableFlag == false)
return;
ros::Rate rate(1);
while (ros::ok()){
rate.sleep();
performLoopClosure();
}
}
3.2 performLoopClosure
void performLoopClosure(){
//1.先进行闭环检测detectLoopClosure()，
//如果返回true,则可能可以进行闭环，否则直接返回，程序结束。
if (cloudKeyPoses3D->points.empty() == true)
return;
if (potentialLoopFlag == false){
if (detectLoopClosure() == true){
potentialLoopFlag = true;
timeSaveFirstCurrentScanForLoopClosure = timeLaserOdometry;
}
if (potentialLoopFlag == false)
return;
}
potentialLoopFlag = false;
//2.接着使用icp迭代进行对齐
pcl::IterativeClosestPoint icp;
icp.setMaxCorrespondenceDistance(100);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
// 设置RANSAC运行次数
icp.setRANSACIterations(0);
icp.setInputSource(latestSurfKeyFrameCloud);
// 使用detectLoopClosure()函数中,下采样刚刚更新nearHistorySurfKeyFrameCloudDS
icp.setInputTarget(nearHistorySurfKeyFrameCloudDS);
pcl::PointCloud::Ptr unused_result(new pcl::PointCloud());
// 进行icp点云对齐
icp.align(*unused_result);
//3.对齐之后判断迭代是否收敛以及噪声是否太大，是则返回并直接结束函数。否则进行迭代后的数据发布处理。
// 为什么匹配分数高直接返回???------分数高代表噪声太多
if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
return;
//4.接下来得到latestSurfKeyFrameCloud和nearHistorySurfKeyFrameCloudDS之间的位置平移和旋转。
// 以下在点云icp收敛并且噪声量在一定范围内进行
if (pubIcpKeyFrames.getNumSubscribers() != 0){
pcl::PointCloud::Ptr closed_cloud(new pcl::PointCloud());
// icp.getFinalTransformation()的返回值是Eigen::Matrix
pcl::transformPointCloud (*latestSurfKeyFrameCloud, *closed_cloud, icp.getFinalTransformation());
sensor_msgs::PointCloud2 cloudMsgTemp;
pcl::toROSMsg(*closed_cloud, cloudMsgTemp);
cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
cloudMsgTemp.header.frame_id = "/camera_init";
pubIcpKeyFrames.publish(cloudMsgTemp);
}
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f correctionCameraFrame;
correctionCameraFrame = icp.getFinalTransformation();
// 得到平移和旋转的角度
pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);
Eigen::Affine3f correctionLidarFrame = pcl::getTransformation(z, x, y, yaw, roll, pitch);
Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar(cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;
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[closestHistoryFrameID]);
gtsam::Vector Vector6(6);
float noiseScore = icp.getFitnessScore();
Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
constraintNoise = noiseModel::Diagonal::Variances(Vector6);
//5.然后进行图优化过程。
std::lock_guard lock(mtx);
gtSAMgraph.add(BetweenFactor(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise));
isam->update(gtSAMgraph);
isam->update();
gtSAMgraph.resize(0);
aLoopIsClosed = true;
}
4.visualizeGlobalMapThread可视化
4.1visualizeMapThread
void visualizeGlobalMapThread(){
ros::Rate rate(0.2);
while (ros::ok()){
rate.sleep();
publishGlobalMap();///laser_cloud_surround话题
// 1.通过KDTree进行最近邻搜索;
// 2.通过搜索得到的索引放进队列;
// 3.通过两次下采样，减小数据量;
//发布/laser_cloud_surround话题
}
}
5.核心run函数
void 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 lock(mtx);
//mappingProcessInterval是0.3秒，以低频进行建图
if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
timeLastProcessing = timeLaserOdometry;
//把点云坐标均转换到世界坐标系下
transformAssociateToMap();
//由于帧数的频率大于建图的频率，因此需要提取关键帧进行匹配
extractSurroundingKeyFrames();
//降采样匹配以及增加地图点云，回环检测
downsampleCurrentScan();
// 当前扫描进行边缘优化，图优化以及进行LM优化的过程,
//是根据现有地图与最新点云数据进行配准,从而更新机器人精确位姿与融合建图，它分为角点优化、平面点优化、配准与更新等部分。
scan2MapOptimization();
//保存轨迹与位姿图,为回环检测做准备
saveKeyFramesAndFactor();
//correctPoses是回环检测成功后将位姿图的数据依次更新
correctPoses();
//发送tf变换
publishTF();
publishKeyPosesAndFrames();
clearCloud();
}
}
}
5.1transformAssociateToMap
transformAssociateToMap将坐标转移到世界坐标系下,得到可用于建图的Lidar坐标，即修改了transformTobeMapped的值
5.2extractSurroundingKeyFrames
void extractSurroundingKeyFrames(){
if (cloudKeyPoses3D->points.empty() == true)
return;
// loopClosureEnableFlag 这个变量另外只在loopthread这部分中有用到
if (loopClosureEnableFlag == true){
// recentCornerCloudKeyFrames保存的点云数量太少，则清空后重新塞入新的点云直至数量够
if (recentCornerCloudKeyFrames.size() < surroundingKeyframeSearchNum){
recentCornerCloudKeyFrames. clear();
recentSurfCloudKeyFrames. clear();
recentOutlierCloudKeyFrames.clear();
int numPoses = cloudKeyPoses3D->points.size();
for (int i = numPoses-1; i >= 0; --i){
// cloudKeyPoses3D的intensity中存的是索引值?
// 保存的索引值从1开始编号；
int thisKeyInd = (int)cloudKeyPoses3D->points[i].intensity;
PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd];
updateTransformPointCloudSinCos(&thisTransformation);
// 依据上面得到的变换thisTransformation，对cornerCloudKeyFrames，surfCloudKeyFrames，surfCloudKeyFrames
// 进行坐标变换
recentCornerCloudKeyFrames. push_front(transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));
recentSurfCloudKeyFrames. push_front(transformPointCloud(surfCloudKeyFrames[thisKeyInd]));
recentOutlierCloudKeyFrames.push_front(transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));
if (recentCornerCloudKeyFrames.size() >= surroundingKeyframeSearchNum)
break;
}
}else{
// recentCornerCloudKeyFrames中点云保存的数量较多
// pop队列最前端的一个，再push后面一个
if (latestFrameID != cloudKeyPoses3D->points.size() - 1){
recentCornerCloudKeyFrames. pop_front();
recentSurfCloudKeyFrames. pop_front();
recentOutlierCloudKeyFrames.pop_front();
// 为什么要把recentCornerCloudKeyFrames最前面第一个元素弹出?
// (1个而不是好几个或者是全部)?
latestFrameID = cloudKeyPoses3D->points.size() - 1;
PointTypePose thisTransformation = cloudKeyPoses6D->points[latestFrameID];
updateTransformPointCloudSinCos(&thisTransformation);
recentCornerCloudKeyFrames. push_back(transformPointCloud(cornerCloudKeyFrames[latestFrameID]));
recentSurfCloudKeyFrames. push_back(transformPointCloud(surfCloudKeyFrames[latestFrameID]));
recentOutlierCloudKeyFrames.push_back(transformPointCloud(outlierCloudKeyFrames[latestFrameID]));
}
}
for (int i = 0; i < recentCornerCloudKeyFrames.size(); ++i){
// 两个pcl::PointXYZI相加?
// 注意这里把recentOutlierCloudKeyFrames也加入到了laserCloudSurfFromMap
*laserCloudCornerFromMap += *recentCornerCloudKeyFrames[i];
*laserCloudSurfFromMap += *recentSurfCloudKeyFrames[i];
*laserCloudSurfFromMap += *recentOutlierCloudKeyFrames[i];
}
}else{
// 下面这部分是没有闭环的代码
//
surroundingKeyPoses->clear();
surroundingKeyPosesDS->clear();
kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D);
// 进行半径surroundingKeyframeSearchRadius内的邻域搜索，
// currentRobotPosPoint：需要查询的点，
// pointSearchInd：搜索完的邻域点对应的索引
// pointSearchSqDis：搜索完的每个领域点点与传讯点之间的欧式距离
// 0：返回的邻域个数，为0表示返回全部的邻域点
kdtreeSurroundingKeyPoses->radiusSearch(currentRobotPosPoint, (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis, 0);
for (int i = 0; i < pointSearchInd.size(); ++i)
surroundingKeyPoses->points.push_back(cloudKeyPoses3D->points[pointSearchInd[i]]);
downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
int numSurroundingPosesDS = surroundingKeyPosesDS->points.size();
for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i){
bool existingFlag = false;
for (int j = 0; j < numSurroundingPosesDS; ++j){
// 双重循环，不断对比surroundingExistingKeyPosesID[i]和surroundingKeyPosesDS的点的index
// 如果能够找到一样的，说明存在相同的关键点(因为surroundingKeyPosesDS从cloudKeyPoses3D中筛选而来)
if (surroundingExistingKeyPosesID[i] == (int)surroundingKeyPosesDS->points[j].intensity){
existingFlag = true;
break;
}
}
if (existingFlag == false){
// 如果surroundingExistingKeyPosesID[i]对比了一轮的已经存在的关键位姿的索引后（intensity保存的就是size()）
// 没有找到相同的关键点，那么把这个点从当前队列中删除
// 否则的话，existingFlag为true，该关键点就将它留在队列中
surroundingExistingKeyPosesID. erase(surroundingExistingKeyPosesID. begin() + i);
surroundingCornerCloudKeyFrames. erase(surroundingCornerCloudKeyFrames. begin() + i);
surroundingSurfCloudKeyFrames. erase(surroundingSurfCloudKeyFrames. begin() + i);
surroundingOutlierCloudKeyFrames.erase(surroundingOutlierCloudKeyFrames.begin() + i);
--i;
}
}
// 上一个两重for循环主要用于删除数据，此处的两重for循环用来添加数据
for (int i = 0; i < numSurroundingPosesDS; ++i) {
bool existingFlag = false;
for (auto iter = surroundingExistingKeyPosesID.begin(); iter != surroundingExistingKeyPosesID.end(); ++iter){
// *iter就是不同的cloudKeyPoses3D->points.size(),
// 把surroundingExistingKeyPosesID内没有对应的点放进一个队列里
// 这个队列专门存放周围存在的关键帧，但是和surroundingExistingKeyPosesID的点没有对应的，也就是新的点
if ((*iter) == (int)surroundingKeyPosesDS->points[i].intensity){
existingFlag = true;
break;
}
}
if (existingFlag == true){
continue;
}else{
int thisKeyInd = (int)surroundingKeyPosesDS->points[i].intensity;
PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd];
updateTransformPointCloudSinCos(&thisTransformation);
surroundingExistingKeyPosesID. push_back(thisKeyInd);
surroundingCornerCloudKeyFrames. push_back(transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));
surroundingSurfCloudKeyFrames. push_back(transformPointCloud(surfCloudKeyFrames[thisKeyInd]));
surroundingOutlierCloudKeyFrames.push_back(transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));
}
}
for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i) {
*laserCloudCornerFromMap += *surroundingCornerCloudKeyFrames[i];
*laserCloudSurfFromMap += *surroundingSurfCloudKeyFrames[i];
*laserCloudSurfFromMap += *surroundingOutlierCloudKeyFrames[i];
}
}
// 进行两次下采样
// 最后的输出结果是laserCloudCornerFromMapDS和laserCloudSurfFromMapDS
downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->points.size();
downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->points.size();
}
5.3downsampleCurrentScan
1.下采样laserCloudCornerLast得到laserCloudCornerLastDS；
2.下采样laserCloudSurfLast得到laserCloudSurfLastDS;
3.下采样laserCloudOutlierLast得到laserCloudOutlierLastDS;
4.laserCloudSurfLastDS和laserCloudOutlierLastDS相加，得到laserCloudSurfTotalLast；
5.下采样得到laserCloudSurfTotalLast，得到得到laserCloudSurfTotalLastDS;
5.4scan2MapOptimization
void 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++) {
// 用for循环控制迭代次数，最多迭代10次
laserCloudOri->clear();
coeffSel->clear();
cornerOptimization(iterCount);
surfOptimization(iterCount);
if (LMOptimization(iterCount) == true)
break;
}
// 迭代结束更新相关的转移矩阵
transformUpdate();
}
}
5.5saveKeyFramesAndFactor
void saveKeyFramesAndFactor(){
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){
saveThisKeyFrame = false;
}
if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty())
return;
previousRobotPosPoint = currentRobotPosPoint;
if (cloudKeyPoses3D->points.empty()){
// static Rot3 RzRyRx (double x, double y, double z),Rotations around Z, Y, then X axes
// RzRyRx依次按照z(transformTobeMapped[2])，y(transformTobeMapped[0])，x(transformTobeMapped[1])坐标轴旋转
// Point3 (double x, double y, double z) Construct from x(transformTobeMapped[5]), y(transformTobeMapped[3]), and z(transformTobeMapped[4]) coordinates.
// Pose3 (const Rot3 &R, const Point3 &t) Construct from R,t. 从旋转和平移构造姿态
// NonlinearFactorGraph增加一个PriorFactor因子
gtSAMgraph.add(PriorFactor(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise));
// initialEstimate的数据类型是Values,其实就是一个map，这里在0对应的值下面保存了一个Pose3
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]));
// 构造函数原型:BetweenFactor (Key key1, Key key2, const VALUE &measured, const SharedNoiseModel &model)
gtSAMgraph.add(BetweenFactor(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])));
}
// gtsam::ISAM2::update函数原型:
// ISAM2Result gtsam::ISAM2::update ( const NonlinearFactorGraph & newFactors = NonlinearFactorGraph(),
// const Values & newTheta = Values(),
// const std::vector< size_t > & removeFactorIndices = std::vector(),
// const boost::optional< FastMap< Key, int > > & constrainedKeys = boost::none,
// const boost::optional< FastList< Key > > & noRelinKeys = boost::none,
// const boost::optional< FastList< Key > > & extraReelimKeys = boost::none,
// bool force_relinearize = false )
// gtSAMgraph是新加到系统中的因子
// initialEstimate是加到系统中的新变量的初始点
isam->update(gtSAMgraph, initialEstimate);
// update 函数为什么需要调用两次？
isam->update();
// 删除内容?
gtSAMgraph.resize(0);
initialEstimate.clear();
PointType thisPose3D;
PointTypePose thisPose6D;
Pose3 latestEstimate;
// Compute an estimate from the incomplete linear delta computed during the last update.
isamCurrentEstimate = isam->calculateEstimate();
latestEstimate = isamCurrentEstimate.at(isamCurrentEstimate.size()-1);
thisPose3D.x = latestEstimate.translation().y();
thisPose3D.y = latestEstimate.translation().z();
thisPose3D.z = latestEstimate.translation().x();
thisPose3D.intensity = cloudKeyPoses3D->points.size();
cloudKeyPoses3D->push_back(thisPose3D);
thisPose6D.x = thisPose3D.x;
thisPose6D.y = thisPose3D.y;
thisPose6D.z = thisPose3D.z;
thisPose6D.intensity = thisPose3D.intensity;
thisPose6D.roll = latestEstimate.rotation().pitch();
thisPose6D.pitch = latestEstimate.rotation().yaw();
thisPose6D.yaw = latestEstimate.rotation().roll();
thisPose6D.time = timeLaserOdometry;
cloudKeyPoses6D->push_back(thisPose6D);
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::Ptr thisCornerKeyFrame(new pcl::PointCloud());
pcl::PointCloud::Ptr thisSurfKeyFrame(new pcl::PointCloud());
pcl::PointCloud::Ptr thisOutlierKeyFrame(new pcl::PointCloud());
// PCL::copyPointCloud(const pcl::PCLPointCloud2 &cloud_in,pcl::PCLPointCloud2 &cloud_out )
pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
pcl::copyPointCloud(*laserCloudOutlierLastDS, *thisOutlierKeyFrame);
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);
}
5.6correctPoses
void correctPoses(){
if (aLoopIsClosed == true){
recentCornerCloudKeyFrames. clear();
recentSurfCloudKeyFrames. clear();
recentOutlierCloudKeyFrames.clear();
int numPoses = isamCurrentEstimate.size();
for (int i = 0; i < numPoses; ++i)
{
cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at(i).translation().y();
cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at(i).translation().z();
cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at(i).translation().x();
cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
//
cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at(i).rotation().pitch();
cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at(i).rotation().yaw();
cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at(i).rotation().roll();
}
aLoopIsClosed = false;
}
}