基于先验地图的全局定位(平移和旋转)
在下面的只有平移的基础上,定位的效果还是非常不错的
附赠自动驾驶最全的学习资料和量产经验:链接
在平移基础上增添旋转时:明明实际运动向左侧行走,实际却是向右侧行走进而导致轨迹出现了问题…
----------------------------------------打印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中的坐标系描述还是不清晰,比如三部分,还需要重写