transformMaintenance.cpp解析
loam源码地址： https://github.com/cuitaixiang/LOAM_NOTED.
论文学习： LOAM: Lidar Odometry and Mapping in Real-time 论文阅读.
loam源码解析汇总：
loam源码解析1 ： scanRegistration（一）.
loam源码解析2 ： scanRegistration（二）.
loam源码解析3 ： laserOdometry（一）.
loam源码解析4 ： laserOdometry（二）.
loam源码解析5 ： laserOdometry（三）.
loam源码解析6 ： laserMapping（一）.
loam源码解析7 ： laserMapping（二）.
loam源码解析8 ： transformMaintenance.
一、概述
到这里，我们已经分析laserOdometry中lidar里程计的工作，也就完成了loam两大算法的其中一个。然而，上一部分Lidar里程计的结果不准确，由它拼起来的点云完全不成样子，而且Lidar里程计会不断发散，因此误差也会越来越大。所以这一节主要是分析loam的另一大算法：建图算法（laserOdometry），通过将多帧的激光特征点云基于laserOdometry提供的pose拼接,形成特征点云地图。（包含corner和surface两种特征点云地图），将新入的帧与地图作配准,得到更精准的pose.相当于scan-to-map，我们又将这个pose将其加入到全局地图中。
二、变量说明
扫描周期：
const float scanPeriod = 0.1;
处理周期：
//控制接收到的点云数据，每隔几帧处理一次
const int stackFrameNum = 1;
//控制处理得到的点云map，每隔几次publich给rviz显示
const int mapFrameNum = 5;
接受全部点云、比较大曲率点、比较小曲率点、里程计的时间戳：
double timeLaserCloudCornerLast = 0;
double timeLaserCloudSurfLast = 0;
double timeLaserCloudFullRes = 0;
double timeLaserOdometry = 0;
接受全部点云、比较大曲率点、比较小曲率点、里程计的标志：
//接收标志
bool newLaserCloudCornerLast = false;
bool newLaserCloudSurfLast = false;
bool newLaserCloudFullRes = false;
bool newLaserOdometry = false;
cube的宽、高、深度信息，以及点云的分割cube个数
int laserCloudCenWidth = 10;
int laserCloudCenHeight = 5;
int laserCloudCenDepth = 10;
const int laserCloudWidth = 21;
const int laserCloudHeight = 11;
const int laserCloudDepth = 21;
//点云方块集合最大数量
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;//4851
点云集索引：
//lidar视域范围内(FOV)的点云集索引
int laserCloudValidInd[125];
//lidar周围的点云集索引
int laserCloudSurroundInd[125];
存放各种点云的定义：
//最新接收到的边沿点
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
//最新接收到的平面点
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());
//存放当前收到的下采样之后的边沿点(in the local frame)
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
//存放当前收到的下采样之后的平面点(in the local frame)
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
//存放当前收到的边沿点，作为下采样的数据源
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack2(new pcl::PointCloud<PointType>());
//存放当前收到的平面点，作为下采样的数据源
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack2(new pcl::PointCloud<PointType>());
//原始点云坐标
pcl::PointCloud<PointType>::Ptr laserCloudOri(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr coeffSel(new pcl::PointCloud<PointType>());
//匹配使用的特征点（下采样之后的）
pcl::PointCloud<PointType>::Ptr laserCloudSurround(new pcl::PointCloud<PointType>());
//匹配使用的特征点（下采样之前的）
pcl::PointCloud<PointType>::Ptr laserCloudSurround2(new pcl::PointCloud<PointType>());
//map中提取的匹配使用的边沿点
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap(new pcl::PointCloud<PointType>());
//map中提取的匹配使用的平面点
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap(new pcl::PointCloud<PointType>());
//点云全部点
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());
//array都是以50米为单位的立方体地图，运行过程中会一直保存(有需要的话可考虑优化，只保存近邻的，或者直接数组开小一点)
//存放边沿点的cube
pcl::PointCloud<PointType>::Ptr laserCloudCornerArray[laserCloudNum];
//存放平面点的cube
pcl::PointCloud<PointType>::Ptr laserCloudSurfArray[laserCloudNum];
//中间变量，存放下采样过的边沿点
pcl::PointCloud<PointType>::Ptr laserCloudCornerArray2[laserCloudNum];
//中间变量，存放下采样过的平面点
pcl::PointCloud<PointType>::Ptr laserCloudSurfArray2[laserCloudNum];
//kd-tree
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN<PointType>());
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN<PointType>());
变换矩阵：
/*************高频转换量**************/
//odometry计算得到的到世界坐标系下的转移矩阵
float transformSum[6] = {0};
//转移增量，只使用了后三个平移增量
float transformIncre[6] = {0};
/*************低频转换量*************/
//以起始位置为原点的世界坐标系下的转换矩阵（猜测与调整的对象）
float transformTobeMapped[6] = {0};
//存放mapping之前的Odometry计算的世界坐标系的转换矩阵（注：低频量，不一定与transformSum一样）
float transformBefMapped[6] = {0};
//存放mapping之后的经过mapping微调之后的转换矩阵
float transformAftMapped[6] = {0};
imu变量：
int imuPointerFront = 0;
int imuPointerLast = -1;
const int imuQueLength = 200;
double imuTime[imuQueLength] = {0};
float imuRoll[imuQueLength] = {0};
float imuPitch[imuQueLength] = {0};
三、预处理
1. 接受来自laserOdometry的ROS消息
在mian函数的一开始，就接受来自laserOdometry的消息并作相应的解析：
int main(int argc, char** argv)
{
ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh;
ros::Subscriber subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_corner_last", 2, laserCloudCornerLastHandler);
ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_surf_last", 2, laserCloudSurfLastHandler);
ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>
("/laser_odom_to_init", 5, laserOdometryHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>
("/velodyne_cloud_3", 2, laserCloudFullResHandler);
ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);
接下来我们就分别查看每个订阅的消息的回调函数。可以看出来，其实每个函数都相对类似，主要是将ROS的消息格式转换。
(1) laserCloudCornerLastHandler
接受边缘点（曲率较大的点）的回调函数
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudCornerLast2)
{
timeLaserCloudCornerLast = laserCloudCornerLast2->header.stamp.toSec();
laserCloudCornerLast->clear();
pcl::fromROSMsg(*laserCloudCornerLast2, *laserCloudCornerLast);
newLaserCloudCornerLast = true;
}
(2) laserCloudSurfLastHandler
接受平面点（曲率较小的点）的回调函数
void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudSurfLast2)
{
timeLaserCloudSurfLast = laserCloudSurfLast2->header.stamp.toSec();
laserCloudSurfLast->clear();
pcl::fromROSMsg(*laserCloudSurfLast2, *laserCloudSurfLast);
newLaserCloudSurfLast = true;
}
(3) laserCloudFullResHandler
接受全部点云的回调函数
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2)
{
timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec();
laserCloudFullRes->clear();
pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes);
newLaserCloudFullRes = true;
}
(4) laserOdometryHandler
接受里程计信息（包括旋转和平移）的回调函数
//接收旋转平移信息
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
timeLaserOdometry = laserOdometry->header.stamp.toSec();
double roll, pitch, yaw;
//四元数转换为欧拉角
geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
transformSum[0] = -pitch;
transformSum[1] = -yaw;
transformSum[2] = roll;
transformSum[3] = laserOdometry->pose.pose.position.x;
transformSum[4] = laserOdometry->pose.pose.position.y;
transformSum[5] = laserOdometry->pose.pose.position.z;
newLaserOdometry = true;
}
2. 定义消息发布
ROS消息发布：
ros::Publisher pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_surround", 1);
ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>
("/velodyne_cloud_registered", 2);
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init", 5);
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
tf形式发布：
tf::TransformBroadcaster tfBroadcaster;
tf::StampedTransform aftMappedTrans;
aftMappedTrans.frame_id_ = "/camera_init";
aftMappedTrans.child_frame_id_ = "/aft_mapped";
3. 数据初始化
定义特征点及特征点索引vector：
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
PointType pointOri, pointSel, pointProj, coeff;
定义优化矩阵：
cv::Mat matA0(5, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matB0(5, 1, CV_32F, cv::Scalar::all(-1));
cv::Mat matX0(3, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));
定义退化预测矩阵：
bool isDegenerate = false;
cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));
建VoxelGrid滤波器，确定体素大小：
//创建VoxelGrid滤波器（体素栅格滤波器）
pcl::VoxelGrid<PointType> downSizeFilterCorner;
//设置体素大小
downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);
pcl::VoxelGrid<PointType> downSizeFilterSurf;
downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);
pcl::VoxelGrid<PointType> downSizeFilterMap;
downSizeFilterMap.setLeafSize(0.6, 0.6, 0.6);
初始化cube：
//指针初始化
for (int i = 0; i < laserCloudNum; i++) {
laserCloudCornerArray[i].reset(new pcl::PointCloud<PointType>());
laserCloudSurfArray[i].reset(new pcl::PointCloud<PointType>());
laserCloudCornerArray2[i].reset(new pcl::PointCloud<PointType>());
laserCloudSurfArray2[i].reset(new pcl::PointCloud<PointType>());
}
调帧频率控制：
int frameCount = stackFrameNum - 1; //0
int mapFrameCount = mapFrameNum - 1; //4
执行周期：
ros::Rate rate(100);
bool status = ros::ok();
确保同步额的收到每帧信息：
while (status) {
ros::spinOnce();
if (newLaserCloudCornerLast && newLaserCloudSurfLast && newLaserCloudFullRes && newLaserOdometry &&
fabs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
fabs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
fabs(timeLaserCloudFullRes - timeLaserOdometry) < 0.005) {
newLaserCloudCornerLast = false;
newLaserCloudSurfLast = false;
newLaserCloudFullRes = false;
newLaserOdometry = false;
四、transformAssociateToMap初始化位姿 T ˉ k W ( t k + 1 ) \bar T_{k}^W(t_{k+1}) TˉkW(tk+1)
基于匀速模型，根据上次微调的结果 T m a p ( t k ) T_{map}(t_{k}) Tmap(tk)和odometry这次与上次计算的结果 T c u r W ( t k + 1 ) T_{{cur}}^W(t_{k+1}) TcurW(tk+1)、 T c u r W ( t k ) T_{{cur}}^W(t_{k}) TcurW(tk)，猜测一个新的世界坐标系的转换矩阵 T ˉ k W ( t k + 1 ) \bar T_{k}^W(t_{k+1}) TˉkW(tk+1)
1. 计算位移增量
transformBefMapped中储存的是上一时刻odometry解算的值，计算上一时刻和这时刻odometry的增量，由于两个时刻的位姿都是基于起始时刻的，所以需要投影到当前时刻。即 δ T c u r ( t k + 1 ) = ( T c u r W ( t k + 1 ) ) − 1 ∗ ( T c u r W ( t k + 1 ) − T c u r W ( t k ) ) \delta T_{cur}(t_{k+1})=(T^W_{cur}(t_{k+1}))^{-1}*(T_{{cur}}^W(t_{k+1})-T_{{cur}}^W(t_{k})) δTcur(tk+1)=(TcurW(tk+1))−1∗(TcurW(tk+1)−TcurW(tk))。
void transformAssociateToMap()
{
float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3])
- sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
float y1 = transformBefMapped[4] - transformSum[4];
float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3])
+ cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
float x2 = x1;
float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;
float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;
//平移增量
transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;
transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;
transformIncre[5] = z2;
2. 修正旋转增量
其原理与loam源码解析3 ： laserOdometry（一）中修正当前点欧拉角PluginIMURotation的方法类似，其公式为： δ R m a p ( t k + 1 ) = R m a p ( t k ) ∗ ( R c u r m a p ( t k ) ) − 1 ∗ δ R c u r ( t k + 1 ) \delta R_{map}(t_{k+1})=R_{map}(t_{k})*(R_{cur}^{map}(t_{k}))^{-1}*\delta R_{cur}(t_{k+1}) δRmap(tk+1)=Rmap(tk)∗(Rcurmap(tk))−1∗δRcur(tk+1)。
float sbcx = sin(transformSum[0]);
float cbcx = cos(transformSum[0]);
float sbcy = sin(transformSum[1]);
float cbcy = cos(transformSum[1]);
float sbcz = sin(transformSum[2]);
float cbcz = cos(transformSum[2]);
float sblx = sin(transformBefMapped[0]);
float cblx = cos(transformBefMapped[0]);
float sbly = sin(transformBefMapped[1]);
float cbly = cos(transformBefMapped[1]);
float sblz = sin(transformBefMapped[2]);
float cblz = cos(transformBefMapped[2]);
float salx = sin(transformAftMapped[0]);
float calx = cos(transformAftMapped[0]);
float saly = sin(transformAftMapped[1]);
float caly = cos(transformAftMapped[1]);
float salz = sin(transformAftMapped[2]);
float calz = cos(transformAftMapped[2]);
float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz)
- cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
- cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx);
transformTobeMapped[0] = -asin(srx);
float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly)
- cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx)
- cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz)
+ (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly)
+ cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz)
+ (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly);
float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz)
- cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx)
+ cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)
+ (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly)
- cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly)
+ (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly);
transformTobeMapped[1] = atan2(srycrx / cos(transformTobeMapped[0]),
crycrx / cos(transformTobeMapped[0]));
float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)
- (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
+ cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);
float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
- (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)
+ cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);
transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[0]),
crzcrx / cos(transformTobeMapped[0]));
3. 修正平移增量
利用刚刚得到的 δ R m a p ( t k + 1 ) \delta R_{map}(t_{k+1}) δRmap(tk+1)修正平移增量:
x1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) * transformIncre[4];
y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2]) * transformIncre[4];
z1 = transformIncre[5];
x2 = x1;
y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;
z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;
transformTobeMapped[3] = transformAftMapped[3]
- (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2);
transformTobeMapped[4] = transformAftMapped[4] - y2;
transformTobeMapped[5] = transformAftMapped[5]
- (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2);
}
五、粗略拼接地图 Q ˉ k \bar Q_k Qˉk
根据论文分析，我们假设 k k k时刻世界坐标系下的位姿为 T k − 1 W ( t k ) T_{k-1}^W(t_{k}) Tk−1W(tk)，通过里程计的信息我们可以得到在 [ t k , t k + 1 ] [t_k,t_{k+1}] [tk,tk+1]内，相对运动为 T k L ( t k + 1 ) T^L_k(t_{k+1}) TkL(tk+1)，点云为 P ˉ k \bar P_k Pˉk,所以我们可以得到一个粗略的 k + 1 k+1 k+1时刻世界坐标系下的位姿为 T ˉ k W ( t k + 1 ) \bar T_{k}^W(t_{k+1}) TˉkW(tk+1)，并将 P ˉ k \bar P_k Pˉk转换到世界坐标系下，得到 Q ˉ k \bar Q_k Qˉk。具体实现如下：
1. 计算粗略的 k + 1 k+1 k+1时刻世界坐标系下的位姿为 T ˉ k W ( t k + 1 ) \bar T_{k}^W(t_{k+1}) TˉkW(tk+1)：
frameCount++;
//控制跳帧数，>=这里实际并没有跳帧，只取>或者增大stackFrameNum才能实现相应的跳帧处理
if (frameCount >= stackFrameNum) {
//获取世界坐标系转换矩阵
transformAssociateToMap();
2. transformAssociateToMap位姿变换
这个函数的实现是为下一小节的转换作准备，利用得到的 T ˉ k W ( t k + 1 ) \bar T_{k}^W(t_{k+1}) TˉkW(tk+1)将点云转换到地图坐标下。
oid pointAssociateToMap(PointType const * const pi, PointType * const po)
{
//绕z轴旋转（transformTobeMapped[2]）
float x1 = cos(transformTobeMapped[2]) * pi->x
- sin(transformTobeMapped[2]) * pi->y;
float y1 = sin(transformTobeMapped[2]) * pi->x
+ cos(transformTobeMapped[2]) * pi->y;
float z1 = pi->z;
//绕x轴旋转（transformTobeMapped[0]）
float x2 = x1;
float y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;
float z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;
//绕y轴旋转（transformTobeMapped[1]），再平移
po->x = cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2
+ transformTobeMapped[3];
po->y = y2 + transformTobeMapped[4];
po->z = -sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2
+ transformTobeMapped[5];
po->intensity = pi->intensity;
}
3. 利用粗略的 k + 1 k+1 k+1时刻地图坐标系下的位姿将点云转换到地图坐标系下
利用粗略的 k + 1 k+1 k+1时刻世界坐标系下的位姿将边缘点转换到世界坐标系下：
//将最新接收到的平面点和边沿点进行旋转平移转换到世界坐标系下(这里和后面的逆转换应无必要)
int laserCloudCornerLastNum = laserCloudCornerLast->points.size();
for (int i = 0; i < laserCloudCornerLastNum; i++) {
pointAssociateToMap(&laserCloudCornerLast->points[i], &pointSel);
laserCloudCornerStack2->push_back(pointSel);
}
利用粗略的 k + 1 k+1 k+1时刻世界坐标系下的位姿将平面点转换到世界坐标系下：
int laserCloudSurfLastNum = laserCloudSurfLast->points.size();
for (int i = 0; i < laserCloudSurfLastNum; i++) {
pointAssociateToMap(&laserCloudSurfLast->points[i], &pointSel);
laserCloudSurfStack2->push_back(pointSel);
}
}
1771
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
