基于先验地图的全局定位(平移和旋转)
在下面的只有平移的基础上，定位的效果还是非常不错的
附赠自动驾驶最全的学习资料和量产经验：链接
在平移基础上增添旋转时：明明实际运动向左侧行走，实际却是向右侧行走进而导致轨迹出现了问题…
----------------------------------------打印log，分析原因
经过查看得知：yaw的初始值没写进初始值弧度1.57
I20240719 11:48:43.987738 27989 localization_todo.cpp:1150] lastIncreOdomPubFlag为false时 yaw=0
源代码如下所示：
if (lastIncreOdomPubFlag == false)
{
lastIncreOdomPubFlag = true;
laserOdomIncremental = laserOdometryROS;
ncreOdomAffine = trans2Affine3f(transformTobeMapped);// 当前下的初始绝对位姿（相对于建图原点）
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(increOdomAffine, x, y, z, roll, pitch, yaw);
LOG(INFO)<<"lastIncreOdomPubFlag为false时 yaw="<<yaw<<endl;
}
在localization.cpp文件中查找得知:transformTobeMapped[2] = cloudInfo.imuYawInit;是后者对前者再次赋值，导致allocateMemory()函数中已初始化的transformTobeMapped[2]值被再次更改了; 需再查关键词imuYawInit
if (cloudKeyPoses3D->points.empty())
{
transformTobeMapped[0] = cloudInfo.imuRollInit;
transformTobeMapped[1] = cloudInfo.imuPitchInit;
transformTobeMapped[2] = cloudInfo.imuYawInit;
if (setHeadingInitialization)
std::cout<<" transformTobeMapped[2]= "<< transformTobeMapped[2] <<std::endl;
LOG(INFO)<<"setHeadingInitialization init_yaw= "<< init_yaw <<std::endl;
lastImuTransformation=pcl::getTransformation(0,0,0,cloudInfo.imuRollInit,cloudInfo.imuPitchInit,cloudInfo.imuYawInit);
}
观察定位生成的轨迹，用EVO进行显示，如下：
按照上面显示的内容，我觉得是雷达点云没利用上给定的初始旋转角yaw=1.57弧度；
---------------------------------------初步定位是雷达点云，没有旋转到先验的角度
一. 判断初始帧的旋转效果
如果初始帧旋转部队，后续的工作就一定不对，如果是对的，后面有可能是对的，但不保证是对的。
所以初始帧旋转是整个算法正确的前提，但是充分不必要条件！
当前帧雷达点云输入，分成角点和面点，然后分别进行了降采样将点云存储在laserCloudCornerLastDS变量里；然后分成了2个步骤处理：
① 下面的步骤是将降采样的点云进行存储
// 当前帧激光角点、平面点，降采样集合
pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
// 保存特征点降采样集合
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
② 下面是利用当前帧的点云进行优化，然后得出一个非解析解的位姿
void cornerOptimization()
{
updatePointAssociateToMap();
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < laserCloudCornerLastDSNum; i++)
{
PointType pointOri, pointSel, coeff;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
pointOri = laserCloudCornerLastDS->points[i];
......
}
然后将点云保存下来，用pcl_viewer一起查看：
确实初始时候的位置是歪了！正确的姿态应该是下面的状态：所以找到了没有匹配上的原因
雷达没有旋转先验给出的180度，下面的在5m位置没有逆时针旋转的第1帧点云和地图进行匹配的图例作为参考：
进行修改初始点云进行旋转后，成功显示(推荐第3种显示风格，原地图点云淡一些，当前帧点云颜色深一些！
二 . 下面是将当前帧点云进行旋转
void transformLidarPoint()
{
Eigen::Affine3f initialize_affine =trans2Affine3f(transformTobeMapped);
pcl::transformPointCloud(*laserCloudSurfLast, *TranslaserCloudSurfLast, initialize_affine); // 输出的旋转后点云是*TranslaserCloudSurfLast
pcl::transformPointCloud(*laserCloudSurfLast, *TranslaserCloudCornerLast, initialize_affine);
}
明明是旋转了180度，然后2帧点云的效果却是旋转90度的效果：
需要进一步研究为什么？
尝试①
一开始想借助ICP在给定位姿的前提下，将地图1个姿态转到另外1个姿态中去，但是PCL不允许这样，因为只要用ICP或者NDT，它就是将source和target点云进行匹配，然后输出到icp.align(参数1，参数2)函数的第2个参数中，但实际效果显示，无论参数2是否在之前已经被赋予1个先验的矩阵，但实际应用中基本不使用对应的矩阵，而是根据source和target点云匹配后得出的位姿作为输出的位姿结果；
for (size_t i = 0; i < 6; i++)
{transformInTheWorld[i]=0;}
transformInTheWorld[2]=5;
transformInTheWorld[3]=1.57;
Eigen::Affine3f thisPose6DInWorld = trans2Affine3f(transformInTheWorld);
icp.align(*transform, thisPose6DInWorld.matrix());
在尝试①失败后，进行尝试②
注意下面函数的使用方法，哪里是输入点云，哪里是输出点云
#include <pcl/common/transforms.h>
pcl::PointCloud<PointType>::Ptr TranslaserCloudCornerLast(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*laserCloudsource, *TranslaserCloudCornerLast, thisPose6DInWorld);
pcl::io::savePCDFileASCII("/home/deploy/PCL_Example/NDT/transformPointCloud.pcd",*TranslaserCloudCornerLast);
经过上面的函数将旋转矩阵打印出来，得知：
deploy@T14s:~/PCL_Example/NDT/build$ ./transformPointCloud
transformInTheWorld[2] yaw=1.57
transformInTheWorld[3] init_x=5
thisPose6DInWorld.matrix()=
0.000796274 -1 0 5
1 0.000796274 0 0
-0 0 1 0
0 0 0 1
上面的旋转和平移在旋转矩阵上分别显示为：
但实际上旋转了不是180度而是90度
旋转矩阵(Rotation Matrix) — 绕 Z 轴(around Z Axis）
旋转矩阵（Rotation Matrix）的推导及其应用www.cnblogs.com/meteoric_cry/p/7987548.html
后来原因找到了，弧度pai，对应3.1415926是旋转一圈
这个印象是错误的，因为它是只是旋转半圈，2pai才是旋转一圈！
show you the code
三. 为什么上面手写的旋转是不对的？
但是下面这样的转法是典型的错误，除了将3.14弧度"误"以为转1圈外，再有就是上面的转法是不对的，因为原来LIO_SAM中的算法已经将每1帧新来的点云中的特征点转到当前所在的位置上，所以不需要咱们自己再去旋转一次了，所以自己再去将base_link坐标系下的激光点云转到当前地图点时候，是**“画蛇添足”!**
为什么说下面的代码是错误的？
void transformLidarPoint()
{
Eigen::Affine3f initialize_affine =trans2Affine3f(transformTobeMapped);
pcl::transformPointCloud(*laserCloudSurfLast, *TranslaserCloudSurfLast, initialize_affine); // 输出的旋转后点云是*TranslaserCloudSurfLast
pcl::transformPointCloud(*laserCloudSurfLast, *TranslaserCloudCornerLast, initialize_affine);
}
因为updatePointAssociateToMap()函数将给出的transformTobeMapped[6]转到了Affine3f数据形式，然后在surfOptimization()函数中引用了pointAssociateToMap(&pointOri, &pointSel)是将当前点转到了即将要匹配的位置处，隐式使用了全局变量transPointAssociateToMap，具体可详细查看此函数的内部了解！
Eigen::Affine3f transPointAssociateToMap;// 全局变量
void updatePointAssociateToMap()
{
transPointAssociateToMap = trans2Affine3f(transformTobeMapped);
}
void surfOptimization()
{
updatePointAssociateToMap();
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < laserCloudSurfLastDSNum; i++)
{
PointType pointOri, pointSel, coeff;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
pointOri = laserCloudSurfLastDS->points[i];
pointAssociateToMap(&pointOri, &pointSel);
}
}
四. 为什么怀疑是IMU出现了问题，以及如何解决？
先将每来一帧数据就转到本地地图，然后进行匹配得出对应的姿态； 具体的从可视化图中分析原因如下图所示：其中红色表示lidar odom，绿色表示imu raw data，当车辆实际向右侧行驶时候，lidar odom稳定地向右侧行驶时，绿色的IMU居然向左行驶，而且还有跳变，所以目前怀疑是IMU没有经过旋转导致的！
------------------------------------------------写给未来
1. 关于原来LIO_SAM中的坐标系描述还是不清晰，比如三部分，还需要重写
502
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
