门户网站建设工作情况,溧阳企业网站建设价格,班级网站的规划与建设,wordpress 来路插件在研读了论文及开源代码后#xff0c;对LOAM的一些理解做一个整理。
文章#xff1a;Low-drift and real-time lidar odometry and mapping
开源代码#xff1a;https://github.com/daobilige-su/loam_velodyne
系统概述
LOAM的整体思想就是将复杂的SLAM问题分为#x…在研读了论文及开源代码后对LOAM的一些理解做一个整理。
文章Low-drift and real-time lidar odometry and mapping
开源代码https://github.com/daobilige-su/loam_velodyne
系统概述
LOAM的整体思想就是将复杂的SLAM问题分为1. 高频的运动估计 2. 低频的环境建图。 Lidar接收数据首先进行Point Cloud RegistrationLidar Odometry以10Hz的频率进行运动估计和坐标转换Lidar Mapping以1Hz的频率构建三维地图Transform Integration完成位姿的优化。这样并行的结构保证了系统的实时性。
接下来是代码的框架图 整个算法分为四个模块相对于其它直接匹配两个点云的算法LOAM是通过提取特征点进行匹配之后计算坐标变换。具体流程为ScanRegistration 提取特征点并排除瑕点LaserOdometry从特征点中估计运动然后整合数据发送给LaserMappingLaserMapping输出的laser_cloud_surround为地图TransformMaintenance订阅LaserOdometry与LaserMapping发布的Odometry消息对位姿进行融合优化。后面将详细进行说明。
ScanRegistration
这一模块节点主要功能是特征点的提取
一次扫描的点通过曲率值来分类特征点曲率大于阈值的为边缘点特征点曲率小于阈值的为平面点。为了使特征点均匀的分布在环境中将一次扫描划分为4个独立的子区域。每个子区域最多提供2个边缘点和4个平面点。此外将不稳定的特征点瑕点排除。下面将通过代码进行说明。
从主函数开始
int main(int argc, char** argv)
{ros::init(argc, argv, scanRegistration);/** NodeHandle 是节点同ROS系统交流的主要接口* NodeHandle 在构造的时候会完整地初始化本节点 * NodeHandle 析构的时候会关闭此节点*/ros::NodeHandle nh;/** 参数1话题名称 * 参数2信息队列长度 * 参数3回调函数每当一个信息到来的时候这个函数会被调用 * 返回一个ros::Subscriber类的对象当此对象的所有引用都被销毁是本节点将不再是该话题的订阅者 */ // 订阅了velodyne_points和imu/dataros::Subscriber subLaserCloud nh.subscribesensor_msgs::PointCloud2 (/velodyne_points, 2, laserCloudHandler); ros::Subscriber subImu nh.subscribesensor_msgs::Imu (/imu/data, 50, imuHandler);/** 我们通过advertise() 函数指定我们如何在给定的topic上发布信息* 它会触发对ROS master的调用master会记录话题发布者和订阅者* 在advertise()函数执行之后master会通知每一个订阅此话题的节点* 两节点间由此可以建立直接的联系* advertise()会返回一个Publisher对象使用这个对象的publish方法我们就可以在此话题上发布信息* 当返回的Publisher对象的所有引用都被销毁的时候本节点将不再是该话题的发布者* 此函数是一个带模板的函数需要传入具体的类型进行实例化* 传入的类型就是要发布的信息的类型在这里是String* 第一个参数是话题名称* 第二个参数是信息队列的长度相当于信息的一个缓冲区* 在我们发布信息的速度大于处理信息的速度时* 信息会被缓存在先进先出的信息队列里*/// 发布了6个话题velodyne_cloud_2、laser_cloud_sharp、laser_cloud_flat、laser_cloud_less_flat、laser_cloud_less_sharp、imu_transpubLaserCloud nh.advertisesensor_msgs::PointCloud2(/velodyne_cloud_2, 2);pubCornerPointsSharp nh.advertisesensor_msgs::PointCloud2(/laser_cloud_sharp, 2);pubCornerPointsLessSharp nh.advertisesensor_msgs::PointCloud2(/laser_cloud_less_sharp, 2);pubSurfPointsFlat nh.advertisesensor_msgs::PointCloud2(/laser_cloud_flat, 2);pubSurfPointsLessFlat nh.advertisesensor_msgs::PointCloud2(/laser_cloud_less_flat, 2);pubImuTrans nh.advertisesensor_msgs::PointCloud2 (/imu_trans, 5);/*** 它可以保证你指定的回调函数会被调用* 程序执行到spin()后就不调用其他语句了*/ros::spin();return 0;
}
回调函数每当一个信息到来的时候这个函数会被调用 * 返回一个ros::Subscriber类的对象当此对象的所有引用都被销毁是本节点将不再是该话题的订阅者 */ // 订阅了velodyne_points和imu/dataros::Subscriber subLaserCloud nh.subscribesensor_msgs::PointCloud2 (/velodyne_points, 2, laserCloudHandler); ros::Subscriber subImu nh.subscribesensor_msgs::Imu (/imu/data, 50, imuHandler);/** 我们通过advertise() 函数指定我们如何在给定的topic上发布信息* 它会触发对ROS master的调用master会记录话题发布者和订阅者* 在advertise()函数执行之后master会通知每一个订阅此话题的节点* 两节点间由此可以建立直接的联系* advertise()会返回一个Publisher对象使用这个对象的publish方法我们就可以在此话题上发布信息* 当返回的Publisher对象的所有引用都被销毁的时候本节点将不再是该话题的发布者* 此函数是一个带模板的函数需要传入具体的类型进行实例化* 传入的类型就是要发布的信息的类型在这里是String* 第一个参数是话题名称* 第二个参数是信息队列的长度相当于信息的一个缓冲区* 在我们发布信息的速度大于处理信息的速度时* 信息会被缓存在先进先出的信息队列里*/// 发布了6个话题velodyne_cloud_2、laser_cloud_sharp、laser_cloud_flat、laser_cloud_less_flat、laser_cloud_less_sharp、imu_transpubLaserCloud nh.advertisesensor_msgs::PointCloud2(/velodyne_cloud_2, 2);pubCornerPointsSharp nh.advertisesensor_msgs::PointCloud2(/laser_cloud_sharp, 2);pubCornerPointsLessSharp nh.advertisesensor_msgs::PointCloud2(/laser_cloud_less_sharp, 2);pubSurfPointsFlat nh.advertisesensor_msgs::PointCloud2(/laser_cloud_flat, 2);pubSurfPointsLessFlat nh.advertisesensor_msgs::PointCloud2(/laser_cloud_less_flat, 2);pubImuTrans nh.advertisesensor_msgs::PointCloud2 (/imu_trans, 5);/*** 它可以保证你指定的回调函数会被调用* 程序执行到spin()后就不调用其他语句了*/ros::spin();return 0;
}
主函数比较简单订阅了2个节点和发布了6个节点。通过回调函数的处理将处理后的点云重新发出去。主要涉及的函数为
laserCloudHandler与imuHandler。
·laserHandler
laserCloudHandler是这一模块的重点部分主要功能是对接收到的点云进行预处理完成分类。具体分类内容为一是将点云划入不同线中存储二是对其进行特征分类。
首先对收到的点云进行处理
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr laserCloudMsg)
{if (!systemInited) {systemInitCount;if (systemInitCount systemDelay) {// systemDelay 有延时作用保证有imu数据后在调用laserCloudHandlersystemInited true;}return;}std::vectorint scanStartInd(N_SCANS, 0);std::vectorint scanEndInd(N_SCANS, 0);// Lidar的时间戳double timeScanCur laserCloudMsg-header.stamp.toSec();pcl::PointCloudpcl::PointXYZ laserCloudIn;// fromROSmsg(input,cloud1) 转为为模板点云laserCloudInpcl::fromROSMsg(*laserCloudMsg, laserCloudIn);std::vectorint indices;//去除无效值pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);int cloudSize laserCloudIn.points.size();//计算点云的起始角度/终止角度float startOri -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);float endOri -atan2(laserCloudIn.points[cloudSize - 1].y,laserCloudIn.points[cloudSize - 1].x) 2 * M_PI;
接下来的处理是根据角度将点划入不同数组中 for (int i 0; i cloudSize; i) {point.x laserCloudIn.points[i].y;point.y laserCloudIn.points[i].z;point.z laserCloudIn.points[i].x;float angle atan(point.y / sqrt(point.x * point.x point.z * point.z)) * 180 / M_PI;int scanID;int roundedAngle int(angle (angle0.0?-0.5:0.5)); if (roundedAngle 0){scanID roundedAngle;}else {// 角度大于0由小到大划入偶数线0-16角度小于0由大到小划入奇数线15-1scanID roundedAngle (N_SCANS - 1);}if (scanID (N_SCANS - 1) || scanID 0 ){// 不在16线附近的点作为杂点进行剔除count--;continue;}接下来计算每个点的相对方位角计算出相对时间根据线性插值的方法计算速度及角度并转换到sweep k的初始imu坐标系下再划入16线数组中 float ori -atan2(point.x, point.z);if (!halfPassed) {if (ori startOri - M_PI / 2) {ori 2 * M_PI;} else if (ori startOri M_PI * 3 / 2) {ori - 2 * M_PI;}if (ori - startOri M_PI) {halfPassed true;}} else {ori 2 * M_PI;if (ori endOri - M_PI * 3 / 2) {ori 2 * M_PI;} else if (ori endOri M_PI / 2) {ori - 2 * M_PI;} }//scanPeriod0.1是因为lidar工作周期是10HZ意味着转一圈是0.1秒intensity是一个整数小数小数不会超过0.1完成了按照时间排序的需求float relTime (ori - startOri) / (endOri - startOri);point.intensity scanID scanPeriod * relTime;// imuPointerLast 是当前点变量只在imu中改变设为t时刻// 对每一个cloud point处理if (imuPointerLast 0) {float pointTime relTime * scanPeriod;while (imuPointerFront ! imuPointerLast) {// (timeScanCur pointTime)设为ti时刻imuPointerFront 是 ti后一个时刻if (timeScanCur pointTime imuTime[imuPointerFront]) {break;}imuPointerFront (imuPointerFront 1) % imuQueLength;}if (timeScanCur pointTime imuTime[imuPointerFront]) {// 这个的意思是 imuPointerFrontimuPointerLast时候imuRollCur imuRoll[imuPointerFront];imuPitchCur imuPitch[imuPointerFront];imuYawCur imuYaw[imuPointerFront];imuVeloXCur imuVeloX[imuPointerFront];imuVeloYCur imuVeloY[imuPointerFront];imuVeloZCur imuVeloZ[imuPointerFront];imuShiftXCur imuShiftX[imuPointerFront];imuShiftYCur imuShiftY[imuPointerFront];imuShiftZCur imuShiftZ[imuPointerFront];} else {// imuPointerBack imuPointerFront - 1 线性插值求解出当前点对应的imu角度位移和速度int imuPointerBack (imuPointerFront imuQueLength - 1) % imuQueLength;float ratioFront (timeScanCur pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);float ratioBack (imuTime[imuPointerFront] - timeScanCur - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);imuRollCur imuRoll[imuPointerFront] * ratioFront imuRoll[imuPointerBack] * ratioBack;imuPitchCur imuPitch[imuPointerFront] * ratioFront imuPitch[imuPointerBack] * ratioBack;if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] M_PI) {imuYawCur imuYaw[imuPointerFront] * ratioFront (imuYaw[imuPointerBack] 2 * M_PI) * ratioBack;} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] -M_PI) {imuYawCur imuYaw[imuPointerFront] * ratioFront (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;} else {imuYawCur imuYaw[imuPointerFront] * ratioFront imuYaw[imuPointerBack] * ratioBack;}imuVeloXCur imuVeloX[imuPointerFront] * ratioFront imuVeloX[imuPointerBack] * ratioBack;imuVeloYCur imuVeloY[imuPointerFront] * ratioFront imuVeloY[imuPointerBack] * ratioBack;imuVeloZCur imuVeloZ[imuPointerFront] * ratioFront imuVeloZ[imuPointerBack] * ratioBack;imuShiftXCur imuShiftX[imuPointerFront] * ratioFront imuShiftX[imuPointerBack] * ratioBack;imuShiftYCur imuShiftY[imuPointerFront] * ratioFront imuShiftY[imuPointerBack] * ratioBack;imuShiftZCur imuShiftZ[imuPointerFront] * ratioFront imuShiftZ[imuPointerBack] * ratioBack;}if (i 0) {imuRollStart imuRollCur;imuPitchStart imuPitchCur;imuYawStart imuYawCur;imuVeloXStart imuVeloXCur;imuVeloYStart imuVeloYCur;imuVeloZStart imuVeloZCur;imuShiftXStart imuShiftXCur;imuShiftYStart imuShiftYCur;imuShiftZStart imuShiftZCur;} else {// 将Lidar位移转到IMU起始坐标系下ShiftToStartIMU(pointTime);// 将Lidar运动速度转到IMU起始坐标系下VeloToStartIMU();// 将点坐标转到起始IMU坐标系下TransformToStartIMU(point);}}// 将点按照每一层线分类压入16个数组中laserCloudScans[scanID].push_back(point);}
之后将对所有点进行曲率值的计算并记录每一层曲率数组的起始和终止
cloudSize count;pcl::PointCloudPointType::Ptr laserCloud(new pcl::PointCloudPointType());//将所有点存入laserCloud中点按线序进行排列for (int i 0; i N_SCANS; i) {*laserCloud laserCloudScans[i];}int scanCount -1;for (int i 5; i cloudSize - 5; i) {// 对所有的激光点一个一个求出在该点前后5个点(10点)的偏差作为cloudCurvature点云数据的曲率float diffX laserCloud-points[i - 5].x laserCloud-points[i - 4].x laserCloud-points[i - 3].x laserCloud-points[i - 2].x laserCloud-points[i - 1].x - 10 * laserCloud-points[i].x laserCloud-points[i 1].x laserCloud-points[i 2].x laserCloud-points[i 3].x laserCloud-points[i 4].x laserCloud-points[i 5].x;float diffY laserCloud-points[i - 5].y laserCloud-points[i - 4].y laserCloud-points[i - 3].y laserCloud-points[i - 2].y laserCloud-fpoints[i - 1].y - 10 * laserCloud-points[i].y laserCloud-points[i 1].y laserCloud-points[i 2].y laserCloud-points[i 3].y laserCloud-points[i 4].y laserCloud-points[i 5].y;float diffZ laserCloud-points[i - 5].z laserCloud-points[i - 4].z laserCloud-points[i - 3].z laserCloud-points[i - 2].z laserCloud-points[i - 1].z - 10 * laserCloud-points[i].z laserCloud-points[i 1].z laserCloud-points[i 2].z laserCloud-points[i 3].z laserCloud-points[i 4].z laserCloud-points[i 5].z;cloudCurvature[i] diffX * diffX diffY * diffY diffZ * diffZ;cloudSortInd[i] i;cloudNeighborPicked[i] 0;cloudLabel[i] 0;if (int(laserCloud-points[i].intensity) ! scanCount) {scanCount int(laserCloud-points[i].intensity);//scanCountscanID// 记录每一层起始点和终止点的位置需要根据这个起始/终止来操作点云曲率在求曲率的过程中已经去除了前5个点和后5个点if (scanCount 0 scanCount N_SCANS) {scanStartInd[scanCount] i 5;scanEndInd[scanCount - 1] i - 5;}}}
接下来对提到的两种瑕点进行排除 for (int i 5; i cloudSize - 6; i) {float diffX laserCloud-points[i 1].x - laserCloud-points[i].x;float diffY laserCloud-points[i 1].y - laserCloud-points[i].y;float diffZ laserCloud-points[i 1].z - laserCloud-points[i].z;float diff diffX * diffX diffY * diffY diffZ * diffZ;if (diff 0.1) {float depth1 sqrt(laserCloud-points[i].x * laserCloud-points[i].x laserCloud-points[i].y * laserCloud-points[i].y laserCloud-points[i].z * laserCloud-points[i].z);float depth2 sqrt(laserCloud-points[i 1].x * laserCloud-points[i 1].x laserCloud-points[i 1].y * laserCloud-points[i 1].y laserCloud-points[i 1].z * laserCloud-points[i 1].z);/*— 针对论文的(b)情况两向量夹角小于某阈值b时夹角小就可能存在遮挡将其一侧的临近6个点设为不可标记为特征点的点 —*//*— 构建了一个等腰三角形的底向量根据等腰三角形性质判断X[i]向量与X[i1]的夹角小于5.732度(threshold0.1) —*//*— depth1depth2 X[i1]距离更近远侧点标记不特征depth1depth2 X[i]距离更近远侧点标记不特征 —*/if (depth1 depth2) {diffX laserCloud-points[i 1].x - laserCloud-points[i].x * depth2 / depth1;diffY laserCloud-points[i 1].y - laserCloud-points[i].y * depth2 / depth1;diffZ laserCloud-points[i 1].z - laserCloud-points[i].z * depth2 / depth1;if (sqrt(diffX * diffX diffY * diffY diffZ * diffZ) / depth2 0.1) {cloudNeighborPicked[i - 5] 1;cloudNeighborPicked[i - 4] 1;cloudNeighborPicked[i - 3] 1;cloudNeighborPicked[i - 2] 1;cloudNeighborPicked[i - 1] 1;cloudNeighborPicked[i] 1;}} else {diffX laserCloud-points[i 1].x * depth1 / depth2 - laserCloud-points[i].x;diffY laserCloud-points[i 1].y * depth1 / depth2 - laserCloud-points[i].y;diffZ laserCloud-points[i 1].z * depth1 / depth2 - laserCloud-points[i].z;if (sqrt(diffX * diffX diffY * diffY diffZ * diffZ) / depth1 0.1) {cloudNeighborPicked[i 1] 1;cloudNeighborPicked[i 2] 1;cloudNeighborPicked[i 3] 1;cloudNeighborPicked[i 4] 1;cloudNeighborPicked[i 5] 1;cloudNeighborPicked[i 6] 1;}}}/*— 针对论文的(a)情况当某点及其后点间的距离平方大于某阈值a说明这两点有一定距离 ———*//*— 若某点到其前后两点的距离均大于c倍的该点深度则该点判定为不可标记特征点的点 ———————*//*—入射角越小点间距越大即激光发射方向与投射到的平面越近似水平 ———————————————*/float diffX2 laserCloud-points[i].x - laserCloud-points[i - 1].x;float diffY2 laserCloud-points[i].y - laserCloud-points[i - 1].y;float diffZ2 laserCloud-points[i].z - laserCloud-points[i - 1].z;float diff2 diffX2 * diffX2 diffY2 * diffY2 diffZ2 * diffZ2;float dis laserCloud-points[i].x * laserCloud-points[i].x laserCloud-points[i].y * laserCloud-points[i].y laserCloud-points[i].z * laserCloud-points[i].z;if (diff 0.0002 * dis diff2 0.0002 * dis) {cloudNeighborPicked[i] 1;}}之后对平面点以及角点进行筛选 pcl::PointCloudPointType cornerPointsSharp;pcl::PointCloudPointType cornerPointsLessSharp;pcl::PointCloudPointType surfPointsFlat;pcl::PointCloudPointType surfPointsLessFlat;for (int i 0; i N_SCANS; i) {/*—— 对于每一层激光点(总16层)将每层区域分成6份起始位置为sp终止位置为ep。——————*//*—— 有两个循环作用是对cloudCurvature从小到大进行排序cloudSortedInd是它的索引数组 ————*/pcl::PointCloudPointType::Ptr surfPointsLessFlatScan(new pcl::PointCloudPointType);for (int j 0; j 6; j) {int sp (scanStartInd[i] * (6 - j) scanEndInd[i] * j) / 6;int ep (scanStartInd[i] * (5 - j) scanEndInd[i] * (j 1)) / 6 - 1;for (int k sp 1; k ep; k) {for (int l k; l sp 1; l--) {if (cloudCurvature[cloudSortInd[l]] cloudCurvature[cloudSortInd[l - 1]]) {int temp cloudSortInd[l - 1];cloudSortInd[l - 1] cloudSortInd[l];cloudSortInd[l] temp;}}}/*—— 筛选特征角点 Corner: label2; LessCorner: label1 ————*/ int largestPickedNum 0;for (int k ep; k sp; k--) {int ind cloudSortInd[k];if (cloudNeighborPicked[ind] 0 cloudCurvature[ind] 0.1) {largestPickedNum;if (largestPickedNum 2) {cloudLabel[ind] 2;cornerPointsSharp.push_back(laserCloud-points[ind]);cornerPointsLessSharp.push_back(laserCloud-points[ind]);} else if (largestPickedNum 20) {cloudLabel[ind] 1;cornerPointsLessSharp.push_back(laserCloud-points[ind]);} else {break;}// 遍历该曲率点后将该点标记并将该曲率点附近的前后5个点标记不被选取为特征点cloudNeighborPicked[ind] 1;for (int l 1; l 5; l) {float diffX laserCloud-points[ind l].x - laserCloud-points[ind l - 1].x;float diffY laserCloud-points[ind l].y - laserCloud-points[ind l - 1].y;float diffZ laserCloud-points[ind l].z - laserCloud-points[ind l - 1].z;if (diffX * diffX diffY * diffY diffZ * diffZ 0.05) {break;}cloudNeighborPicked[ind l] 1;}for (int l -1; l -5; l--) {float diffX laserCloud-points[ind l].x - laserCloud-points[ind l 1].x;float diffY laserCloud-points[ind l].y - laserCloud-points[ind l 1].y;float diffZ laserCloud-points[ind l].z - laserCloud-points[ind l 1].z;if (diffX * diffX diffY * diffY diffZ * diffZ 0.05) {break;}cloudNeighborPicked[ind l] 1;}}}/*—— 筛选特征平面点 Flat: label-1 普通点和Flat点降采样形成LessFlat: label0 ————*/int smallestPickedNum 0;for (int k sp; k ep; k) {int ind cloudSortInd[k];if (cloudNeighborPicked[ind] 0 cloudCurvature[ind] 0.1) {cloudLabel[ind] -1;surfPointsFlat.push_back(laserCloud-points[ind]);smallestPickedNum;if (smallestPickedNum 4) {break;}cloudNeighborPicked[ind] 1;for (int l 1; l 5; l) {float diffX laserCloud-points[ind l].x - laserCloud-points[ind l - 1].x;float diffY laserCloud-points[ind l].y - laserCloud-points[ind l - 1].y;float diffZ laserCloud-points[ind l].z - laserCloud-points[ind l - 1].z;if (diffX * diffX diffY * diffY diffZ * diffZ 0.05) {break;}cloudNeighborPicked[ind l] 1;}for (int l -1; l -5; l--) {float diffX laserCloud-points[ind l].x - laserCloud-points[ind l 1].x;float diffY laserCloud-points[ind l].y - laserCloud-points[ind l 1].y;float diffZ laserCloud-points[ind l].z - laserCloud-points[ind l 1].z;if (diffX * diffX diffY * diffY diffZ * diffZ 0.05) {break;}cloudNeighborPicked[ind l] 1;}}}// surfPointsLessFlat为降采样后的flat点采样前包含太多label0的点for (int k sp; k ep; k) {if (cloudLabel[k] 0) {surfPointsLessFlatScan-push_back(laserCloud-points[k]);}}}// 降采样pcl::PointCloudPointType surfPointsLessFlatScanDS;pcl::VoxelGridPointType downSizeFilter;downSizeFilter.setInputCloud(surfPointsLessFlatScan);downSizeFilter.setLeafSize(0.2, 0.2, 0.2);downSizeFilter.filter(surfPointsLessFlatScanDS);surfPointsLessFlat surfPointsLessFlatScanDS;}
之后再将信息发布出去
·imuHandler
imuHandler功能为imu信息的解析 减去重力对imu的影响 解析出当前时刻的imu时间戳角度以及各个轴的加速度 将加速度转换到世界坐标系轴下 进行航距推算假定为匀速运动推算出当前时刻的位置) 推算当前时刻的速度信息
void imuHandler(const sensor_msgs::Imu::ConstPtr imuIn)
{double roll, pitch, yaw;tf::Quaternion orientation;tf::quaternionMsgToTF(imuIn-orientation, orientation);tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);// 减去重力加速度的影响float accX imuIn-linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;float accY imuIn-linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;float accZ imuIn-linear_acceleration.x sin(pitch) * 9.81;// 表明数组是一个长度为imuQueLength的循环数组imuPointerLast (imuPointerLast 1) % imuQueLength;imuTime[imuPointerLast] imuIn-header.stamp.toSec();imuRoll[imuPointerLast] roll;imuPitch[imuPointerLast] pitch;imuYaw[imuPointerLast] yaw;imuAccX[imuPointerLast] accX;imuAccY[imuPointerLast] accY;imuAccZ[imuPointerLast] accZ;AccumulateIMUShift();
}
void AccumulateIMUShift()
{float roll imuRoll[imuPointerLast];float pitch imuPitch[imuPointerLast];float yaw imuYaw[imuPointerLast];float accX imuAccX[imuPointerLast];float accY imuAccY[imuPointerLast];float accZ imuAccZ[imuPointerLast];// 转换到世界坐标系下float x1 cos(roll) * accX - sin(roll) * accY;float y1 sin(roll) * accX cos(roll) * accY;float z1 accZ;float x2 x1;float y2 cos(pitch) * y1 - sin(pitch) * z1;float z2 sin(pitch) * y1 cos(pitch) * z1;accX cos(yaw) * x2 sin(yaw) * z2;accY y2;accZ -sin(yaw) * x2 cos(yaw) * z2;// imuPointerBack 为 imuPointerLast-1 这样处理是为了防止内存溢出int imuPointerBack (imuPointerLast imuQueLength - 1) % imuQueLength;double timeDiff imuTime[imuPointerLast] - imuTime[imuPointerBack];if (timeDiff scanPeriod) {// 推算当前时刻的位置信息imuShiftX[imuPointerLast] imuShiftX[imuPointerBack] imuVeloX[imuPointerBack] * timeDiff accX * timeDiff * timeDiff / 2;imuShiftY[imuPointerLast] imuShiftY[imuPointerBack] imuVeloY[imuPointerBack] * timeDiff accY * timeDiff * timeDiff / 2;imuShiftZ[imuPointerLast] imuShiftZ[imuPointerBack] imuVeloZ[imuPointerBack] * timeDiff accZ * timeDiff * timeDiff / 2;// 推算当前时刻的速度信息imuVeloX[imuPointerLast] imuVeloX[imuPointerBack] accX * timeDiff;imuVeloY[imuPointerLast] imuVeloY[imuPointerBack] accY * timeDiff;imuVeloZ[imuPointerLast] imuVeloZ[imuPointerBack] accZ * timeDiff;}
}
后面涉及的坐标变换即欧拉角的旋转计算这部分尚不熟悉需要进一步学习。
·小结 以上数据传输流帮助理解这个模块源代码的功能。 ——————————————————————————————————
感谢清酒不是九提出的问题与解决方案
增加以下内容loam在运行较大的图时修改decay time可以让之前的点云不被刷新截掉。地图稠密。
