LOAM学习-代码解析（三）特征点运动估计 laserOdometry

前言
一、初始化
二、去除位移畸变 TransformToStart TransformToEnd
三、去除角度畸变 PluginIMURotation
四、积累旋转量 AccumulateRotation
五、数据处理器 Handler
六、主函数 main
结语

前言

前两篇文章LOAM学习-代码解析（一）点云数据配准和LOAM学习-代码解析（二）点云数据配准 scanRegistration，对点云数据配准的源代码进行详细完整的即系，这里继续LOAM的代码解析工作。

LOAM代码(带中文注释)的地址：https://github.com/cuitaixiang/LOAM_NOTED

LOAM代码(带中文注释)的百度网盘链接：https://pan.baidu.com/s/1tVSNBxNQrxKJyd5c9mWFWw 提取码: wwxr

LOAM论文的百度网盘链接： https://pan.baidu.com/s/10ahqg8O3G2-xOt9QZ1GuEQ 提取码: hnri

LOAM流程：

LOAM学习-代码解析（三）特征点运动估计 laserOdometry前言一、初始化二、去除位移畸变 TransformToStart TransformToEnd三、去除角度畸变 PluginIMURotation四、积累旋转量 AccumulateRotation五、数据处理器 Handler六、主函数 main结语

从LOAM流程图上可一看出，完成点云配准之后，需要进行激光雷达里程计的计算。

一、初始化

设置点云周期，lidar从特征点中估计运动频率为10Hz，因此点云周期为0.1s。

发送数据给laser Mapping的频率为 1HZ，因此设置数据发送周期为1s。

const int skipFrameNum = 1;
bool systemInited = false;

设置时间戳信息

double timeCornerPointsSharp = 0;
double timeCornerPointsLessSharp = 0;
double timeSurfPointsFlat = 0;
double timeSurfPointsLessFlat = 0;
double timeLaserCloudFullRes = 0;
double timeImuTrans = 0;

设置消息接收标志

bool newCornerPointsSharp = false;
bool newCornerPointsLessSharp = false;
bool newSurfPointsFlat = false;
bool newSurfPointsLessFlat = false;
bool newLaserCloudFullRes = false;
bool newImuTrans = false;

接收scanRegistration的信息

//获得尖锐点
pcl::PointCloud<PointType>::Ptr cornerPointsSharp(new pcl::PointCloud<PointType>());
//获得较少的尖锐点
pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp(new pcl::PointCloud<PointType>());
//获得平面点
pcl::PointCloud<PointType>::Ptr surfPointsFlat(new pcl::PointCloud<PointType>());
//获得更少的平面点 
pcl::PointCloud<PointType>::Ptr surfPointsLessFlat(new pcl::PointCloud<PointType>());
//最后一帧的尖锐点
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
//最后一帧的平面点
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(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 laserCloudFullRes(new pcl::PointCloud<PointType>());
//接收imu信息
pcl::PointCloud<pcl::PointXYZ>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZ>());
//用最后一帧的较少尖锐点建立kd树
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<PointType>());
//用最后一帧较少的平面点建立kd树
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<PointType>());

设置计数标志

int laserCloudCornerLastNum;
int laserCloudSurfLastNum;

设置角点和平面点变量

//unused
int pointSelCornerInd[40000];
//保存搜索到的2个角点索引
float pointSearchCornerInd1[40000];
float pointSearchCornerInd2[40000];

//unused
int pointSelSurfInd[40000];
//保存搜索到的3个平面点索引
float pointSearchSurfInd1[40000];
float pointSearchSurfInd2[40000];
float pointSearchSurfInd3[40000];

设置状态转移量

//当前帧相对上一帧的状态转移量，in the local frame
float transform[6] = {0};
//当前帧相对于第一帧的状态转移量，in the global frame
float transformSum[6] = {0};

设置点云的欧拉角、畸变位移、畸变速度

//点云第一个点的RPY
float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0;
//点云最后一个点的RPY
float imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0;
//点云最后一个点相对于第一个点由于加减速产生的畸变位移
float imuShiftFromStartX = 0, imuShiftFromStartY = 0, imuShiftFromStartZ = 0;
//点云最后一个点相对于第一个点由于加减速产生的畸变速度
float imuVeloFromStartX = 0, imuVeloFromStartY = 0, imuVeloFromStartZ = 0;

二、去除位移畸变 TransformToStart TransformToEnd

将当前帧点云TransformToStart和将上一帧点云TransformToEnd的作用：去除畸变，并将两帧点云数据统一到同一个坐标系下计算。

当前点云中的点相对第一个点去除因匀速运动产生的畸变，效果相当于得到在点云扫描开始位置静止扫描得到的点云。

在这一部分中，要回顾scanRegistration.cpp中intensity的计算代码，是由scanID和scanPeriod * relTime两部分组成。而relTime是点旋转的角度与整个周期旋转角度的比率, 即点云中点的相对时间。

// scanRegistration.cpp 371-374
//-0.5 < relTime < 1.5（点旋转的角度与整个周期旋转角度的比率, 即点云中点的相对时间）
float relTime = (ori - startOri) / (endOri - startOri);
//点强度=线号+点相对时间（即一个整数+一个小数，整数部分是线号，小数部分是该点的相对时间）,匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间
//扫描周期, velodyne频率10Hz，周期0.1s,scanPeriod=0.1
point.intensity = scanID + scanPeriod * relTime;

插值系数的计算公式如下，感觉作者这里写的很比较绕，实际上计算的就是点旋转的角度与整个周期旋转角度的比率，即点的相对时间。

s = F p e r i o d ∗ ( I p o i n t − i n t ( I p o i n t ) ) = F p e r i o d ∗ T p e r i o d ∗ T r e l = T r e l s = F_{period}*(I_{point}-int(I_{point})) =F_{period}*T_{period}*T_{rel}=T_{rel} s=Fperiod∗(Ipoint−int(Ipoint))=Fperiod∗Tperiod∗Trel=Trel

s = T r e l = O r i − O r i s t a r t O r i e n d − O r i s t a r t s = T_{rel}=\frac{Ori-Ori_{start}}{Ori_{end}-Ori_{start}} s=Trel=Oriend−OristartOri−Oristart

这里需要重新了解旋转矩阵，由局部坐标系转换到世界坐标系。

[ x y z ] T = [ x p o i n t y p o i n t z p o i n t ] T R z ( γ ) R x ( α ) R y ( β ) \begin{bmatrix} x\\ y\\ z\end{bmatrix}^T=\begin{bmatrix} x_{point}\\ y_{point}\\ z_{point}\end{bmatrix}^TR_z(γ)R_x(α) R_y(β) ⎣⎡xyz⎦⎤T=⎣⎡xpointypointzpoint⎦⎤TRz(γ)Rx(α)Ry(β)

R y ( β ) = [ c o s β 0 s i n β 0 1 0 − s i n β 0 c o s β ] R_y(β) = \begin{bmatrix} cosβ & 0 & sinβ\\ 0 & 1 & 0\\ -sinβ & 0 & cosβ \end{bmatrix} Ry(β)=⎣⎡cosβ0−sinβ010sinβ0cosβ⎦⎤

R x ( α ) = [ 1 0 0 0 c o s α − s i n α 0 s i n α c o s α ] R_x(α)=\begin{bmatrix} 1 & 0 & 0\\ 0 & cosα & -sinα\\ 0 & sinα& cosα \end{bmatrix} Rx(α)=⎣⎡1000cosαsinα0−sinαcosα⎦⎤

R z ( γ ) = [ c o s γ − s i n γ 0 s i n γ c o s γ 0 0 0 1 ] R_z(γ)=\begin{bmatrix} cosγ & -sinγ & 0\\ sinγ & cosγ & 0\\ 0 & 0 & 1 \end{bmatrix} Rz(γ)=⎣⎡cosγsinγ0−sinγcosγ0001⎦⎤

//当前点云中的点相对第一个点去除因匀速运动产生的畸变，效果相当于得到在点云扫描开始位置静止扫描得到的点云
void TransformToStart(PointType const * const pi, PointType * const po)
{
  //插值系数计算，云中每个点的相对时间/点云周期10
  float s = 10 * (pi->intensity - int(pi->intensity));

  //线性插值：根据每个点在点云中的相对位置关系，乘以相应的旋转平移系数
  float rx = s * transform[0];
  float ry = s * transform[1];
  float rz = s * transform[2];
  float tx = s * transform[3];
  float ty = s * transform[4];
  float tz = s * transform[5];

  //平移后绕z轴旋转（-rz）
  float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
  float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
  float z1 = (pi->z - tz);

  //绕x轴旋转（-rx）
  float x2 = x1;
  float y2 = cos(rx) * y1 + sin(rx) * z1;
  float z2 = -sin(rx) * y1 + cos(rx) * z1;

  //绕y轴旋转（-ry）
  po->x = cos(ry) * x2 - sin(ry) * z2;
  po->y = y2;
  po->z = sin(ry) * x2 + cos(ry) * z2;
  po->intensity = pi->intensity;
}

TransformToEnd函数中，需要将点在上一帧时刻局部坐标系转换到世界坐标系，再由世界坐标系转换到上一帧时刻局部坐标系。接着平移imu位移畸变值，再将在局部坐标系转换到世界坐标系，再由世界坐标系转换到局部坐标系。

这里写的代码实在是太绕了，我暂时不是很能理解，为什么要进行四次转换…

//将上一帧点云中的点相对结束位置去除因匀速运动产生的畸变，效果相当于得到在点云扫描结束位置静止扫描得到的点云
void TransformToEnd(PointType const * const pi, PointType * const po)
{
  //插值系数计算
  float s = 10 * (pi->intensity - int(pi->intensity));

  float rx = s * transform[0];
  float ry = s * transform[1];
  float rz = s * transform[2];
  float tx = s * transform[3];
  float ty = s * transform[4];
  float tz = s * transform[5];

  //平移后绕z轴旋转（-rz）
  float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
  float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
  float z1 = (pi->z - tz);

  //绕x轴旋转（-rx）
  float x2 = x1;
  float y2 = cos(rx) * y1 + sin(rx) * z1;
  float z2 = -sin(rx) * y1 + cos(rx) * z1;

  //绕y轴旋转（-ry）
  float x3 = cos(ry) * x2 - sin(ry) * z2;
  float y3 = y2;
  float z3 = sin(ry) * x2 + cos(ry) * z2;//求出了相对于起始点校正的坐标

  rx = transform[0];
  ry = transform[1];
  rz = transform[2];
  tx = transform[3];
  ty = transform[4];
  tz = transform[5];

  //绕y轴旋转（ry）
  float x4 = cos(ry) * x3 + sin(ry) * z3;
  float y4 = y3;
  float z4 = -sin(ry) * x3 + cos(ry) * z3;

  //绕x轴旋转（rx）
  float x5 = x4;
  float y5 = cos(rx) * y4 - sin(rx) * z4;
  float z5 = sin(rx) * y4 + cos(rx) * z4;

  //绕z轴旋转（rz），再平移
  float x6 = cos(rz) * x5 - sin(rz) * y5 + tx;
  float y6 = sin(rz) * x5 + cos(rz) * y5 + ty;
  float z6 = z5 + tz;

  //平移后绕z轴旋转（imuRollStart）
  float x7 = cos(imuRollStart) * (x6 - imuShiftFromStartX) 
           - sin(imuRollStart) * (y6 - imuShiftFromStartY);
  float y7 = sin(imuRollStart) * (x6 - imuShiftFromStartX) 
           + cos(imuRollStart) * (y6 - imuShiftFromStartY);
  float z7 = z6 - imuShiftFromStartZ;

  //绕x轴旋转（imuPitchStart）
  float x8 = x7;
  float y8 = cos(imuPitchStart) * y7 - sin(imuPitchStart) * z7;
  float z8 = sin(imuPitchStart) * y7 + cos(imuPitchStart) * z7;

  //绕y轴旋转（imuYawStart）
  float x9 = cos(imuYawStart) * x8 + sin(imuYawStart) * z8;
  float y9 = y8;
  float z9 = -sin(imuYawStart) * x8 + cos(imuYawStart) * z8;

  //绕y轴旋转（-imuYawLast）
  float x10 = cos(imuYawLast) * x9 - sin(imuYawLast) * z9;
  float y10 = y9;
  float z10 = sin(imuYawLast) * x9 + cos(imuYawLast) * z9;

  //绕x轴旋转（-imuPitchLast）
  float x11 = x10;
  float y11 = cos(imuPitchLast) * y10 + sin(imuPitchLast) * z10;
  float z11 = -sin(imuPitchLast) * y10 + cos(imuPitchLast) * z10;

  //绕z轴旋转（-imuRollLast）
  po->x = cos(imuRollLast) * x11 + sin(imuRollLast) * y11;
  po->y = -sin(imuRollLast) * x11 + cos(imuRollLast) * y11;
  po->z = z11;
  //只保留线号
  po->intensity = int(pi->intensity);
}

三、去除角度畸变 PluginIMURotation

啊这…

应该是使用旋转矩阵进行角度畸变矫正，

//利用IMU修正旋转量，根据起始欧拉角，当前点云的欧拉角修正
void PluginIMURotation(float bcx, float bcy, float bcz, float blx, float bly, float blz, 
                       float alx, float aly, float alz, float &acx, float &acy, float &acz)
{
  float sbcx = sin(bcx);
  float cbcx = cos(bcx);
  float sbcy = sin(bcy);
  float cbcy = cos(bcy);
  float sbcz = sin(bcz);
  float cbcz = cos(bcz);

  float sblx = sin(blx);
  float cblx = cos(blx);
  float sbly = sin(bly);
  float cbly = cos(bly);
  float sblz = sin(blz);
  float cblz = cos(blz);

  float salx = sin(alx);
  float calx = cos(alx);
  float saly = sin(aly);
  float caly = cos(aly);
  float salz = sin(alz);
  float calz = cos(alz);

  float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) 
            - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
            - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
            - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
            - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);
  acx = -asin(srx);

  float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
               - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
               - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
               - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
               + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
  float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
               - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
               - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
               - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
               + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
  acy = atan2(srycrx / cos(acx), crycrx / cos(acx));
  
  float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) 
               - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) 
               - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) 
               + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) 
               - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz 
               + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) 
               + calx*cblx*salz*sblz);
  float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) 
               - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) 
               + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 
               + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) 
               + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly 
               - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) 
               - calx*calz*cblx*sblz);
  acz = atan2(srzcrx / cos(acx), crzcrx / cos(acx));
}

四、积累旋转量 AccumulateRotation

看不明白…

//相对于第一个点云即原点，积累旋转量
void AccumulateRotation(float cx, float cy, float cz, float lx, float ly, float lz, 
                        float &ox, float &oy, float &oz)
{
  float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);
  ox = -asin(srx);

  float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) 
               + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy);
  float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) 
               - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx));
  oy = atan2(srycrx / cos(ox), crycrx / cos(ox));

  float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) 
               + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz);
  float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) 
               - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx));
  oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));
}

五、数据处理器 Handler

这里设置了laserCloudSharp、laserCloudLessSharp、laserCloudFlat、laserCloudLessFlat、laserCloudFullResHandler、imuTransHandler、等数据处理器（Handler）。

//数据处理器
void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharp2)
{
  timeCornerPointsSharp = cornerPointsSharp2->header.stamp.toSec();

  cornerPointsSharp->clear();
  pcl::fromROSMsg(*cornerPointsSharp2, *cornerPointsSharp);
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices);
  newCornerPointsSharp = true;
}

void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharp2)
{
  timeCornerPointsLessSharp = cornerPointsLessSharp2->header.stamp.toSec();

  cornerPointsLessSharp->clear();
  pcl::fromROSMsg(*cornerPointsLessSharp2, *cornerPointsLessSharp);
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*cornerPointsLessSharp,*cornerPointsLessSharp, indices);
  newCornerPointsLessSharp = true;
}

void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlat2)
{
  timeSurfPointsFlat = surfPointsFlat2->header.stamp.toSec();

  surfPointsFlat->clear();
  pcl::fromROSMsg(*surfPointsFlat2, *surfPointsFlat);
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*surfPointsFlat,*surfPointsFlat, indices);
  newSurfPointsFlat = true;
}

void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlat2)
{
  timeSurfPointsLessFlat = surfPointsLessFlat2->header.stamp.toSec();

  surfPointsLessFlat->clear();
  pcl::fromROSMsg(*surfPointsLessFlat2, *surfPointsLessFlat);
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*surfPointsLessFlat,*surfPointsLessFlat, indices);
  newSurfPointsLessFlat = true;
}

//接收全部点
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2)
{
  timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec();

  laserCloudFullRes->clear();
  pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes);
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*laserCloudFullRes,*laserCloudFullRes, indices);
  newLaserCloudFullRes = true;
}

//接收imu消息
void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTrans2)
{
  timeImuTrans = imuTrans2->header.stamp.toSec();

  imuTrans->clear();
  pcl::fromROSMsg(*imuTrans2, *imuTrans);

  //根据发来的消息提取imu信息
  imuPitchStart = imuTrans->points[0].x;
  imuYawStart = imuTrans->points[0].y;
  imuRollStart = imuTrans->points[0].z;

  imuPitchLast = imuTrans->points[1].x;
  imuYawLast = imuTrans->points[1].y;
  imuRollLast = imuTrans->points[1].z;

  imuShiftFromStartX = imuTrans->points[2].x;
  imuShiftFromStartY = imuTrans->points[2].y;
  imuShiftFromStartZ = imuTrans->points[2].z;

  imuVeloFromStartX = imuTrans->points[3].x;
  imuVeloFromStartY = imuTrans->points[3].y;
  imuVeloFromStartZ = imuTrans->points[3].z;

  newImuTrans = true;
}

六、主函数 main

这里的代码非常长，我留到下一章再继续解析吧。

结语

至此，已经把laserOdometry.cpp函数的内容解析一半了。上述内容还有几处不太理解的，如果有人能够解答，就请给我留言吧，十分感谢。

如果你看到这里，说明你已经下定决心要学习loam了，学习新知识的过程总是痛苦的，与君共勉吧！

LOAM学习-代码解析（三）特征点运动估计 laserOdometry前言一、初始化二、去除位移畸变 TransformToStart TransformToEnd三、去除角度畸变 PluginIMURotation四、积累旋转量 AccumulateRotation五、数据处理器 Handler六、主函数 main结语

LOAM学习-代码解析（三）特征点运动估计 laserOdometry

前言

一、初始化

二、去除位移畸变 TransformToStart TransformToEnd

三、去除角度畸变 PluginIMURotation

四、积累旋转量 AccumulateRotation

五、数据处理器 Handler

六、主函数 main

结语

继续阅读

WiFi SLAM包配置记录WiFi SLAM包配置记录然后，按照前面编译顺序，依次往下编译即可，编译完成之后就可以使用wifislam了

ROS笔记：Ubuntu18.04环境下melodic源码安装gmapping

win10+VS2017配置Sophus库win10+VS2017配置Sophus库

学习笔记 -- opencv图像去畸变法一：使用 getOptimalNewCameraMatrix + initUndistortRectifyMap + remap 矫正图像法一完整代码：法二：使用 undistort（）法三：手动实现参考

解读《视觉SLAM十四讲》，带你一步一步入门视觉SLAM—— 第 1 讲预备知识

代码中启动ros节点roslaunch和rosrun,代码启动节点与关闭节点

cartographer从入门到放弃4---核心算法分析---数据处理与Local SLAM

视觉slam线匹配算法

apollo自动驾驶平台配置笔记

粒子滤波学习记录粒子滤波学习记录

国内外LiDAR SLAM实验室总结☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆

基于ORB特征点匹配的对极几何约束实现（源码+讲解）

在Ubuntu16.04上提取相邻序列图像之间的ORB的特征点，并用暴力方法找到匹配点并连线一、什么是ORB特征二、什么是暴力匹配三、实现代码四、运行方法

Turtlebot :" where am I? " -——amcl的原理

视觉传感器在无人驾驶领域取得技术突破

SLAM主要步骤