LIO-SAM 详读代码笔记 -- 3.imageProjection

已于 2022-08-19 19:15:36 修改
阅读量1.7k 收藏 8
点赞数 1
分类专栏： slam 激光SLAM 文章标签： 自动驾驶 人工智能 机器学习
于 2022-08-12 18:30:42 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/fuzi2012/article/details/126309576
版权

讲完上面两部分，我就来到真正的激光雷达数据处理的模块了。剩下有三部分，除imageProjection订阅了IMU源数据、Lidar源数据、ImuPreintegration发过来的odom(里程计数据)之外，就只通过自定义的消息体cloud_info 串联整个激光雷达处理到定位的所有模块(最后的MapOptimization还接收了GPS 源数据、回环检测数据，做因子图优化)，先把系统topic 数据流图搬过来，能更加清晰的理解这个模块。

图2.ros topic 发布与订阅关系图关系图（还是叫图2，因为是同一个图）

图1.代码架构图(数据流图) 分析真正的代码之前，需要先看cloud_info这个消息体的定义。

变量名类型注释备注
headerHeaderros 消息头-
startRingIndexint32[]整形数组，在整个数据结构中，激光点云数据拉平保存在一维数组中，这个数组保存了每一线雷达的第一个数据在这个点云数组中的起始位置-
endRingIndexint32[]这个数组保存了每一线雷达的最后一个数据在这个点云数组中的位置索引-
pointColIndint32[]# point column index in range image 这里保存的是该点云数据在二维映射图像中的列数索引-
pointRangefloat32[]# point range 点云数据对应的深度-
imuAvailableint64标记IMU 数据可用-
odomAvailableint64标记imuPreintegration发来的odom 数据可用-
imuRollInitfloat32保存了当帧点云起始时刻的roll角度-
imuPitchInitfloat32保存了当帧点云起始时刻的pitch角度-
imuYawInitfloat32保存了当帧点云起始时刻的yaw角度-
Initial guess from imu pre-integration-
initialGuessXfloat32保存了当帧点云起始时刻的位姿信息，要给mapOptimization 作位姿初始化用-
initialGuessYfloat32保存了当帧点云起始时刻的位姿信息，要给mapOptimization 作位姿初始化用-
initialGuessZfloat32保存了当帧点云起始时刻的位姿信息，要给mapOptimization 作位姿初始化用-
initialGuessRollfloat32保存了当帧点云起始时刻的位姿信息，要给mapOptimization 作位姿初始化用-
initialGuessPitchfloat32保存了当帧点云起始时刻的位姿信息，要给mapOptimization 作位姿初始化用-
initialGuessYawfloat32保存了当帧点云起始时刻的位姿信息，要给mapOptimization 作位姿初始化用-
Point cloud messages-
cloud_deskewedsensor_msgs/PointCloud2original cloud deskewed 经过去畸变处理的点云数据，封装在这个数据结构中，传递到下个环节环节中去-
cloud_cornersensor_msgs/PointCloud2# extracted corner feature 角点点云数据，这个字段是在featureExtraction模块中填充，传递给mapOptimization模块-
cloud_surfacesensor_msgs/PointCloud2# extracted surface feature 平面点云数据，这个字段是在featureExtraction模块中填充，传递给mapOptimization模块-
3rd party messages-
key_frame_cloudsensor_msgs/PointCloud2待定-
key_frame_colorsensor_msgs/PointCloud2待定-
key_frame_posessensor_msgs/PointCloud2待定-
key_frame_mapsensor_msgs/PointCloud2待定-

imageProjection

先看成员列表：

变量名类型注释备注
imuLockstd::muteximu 数据队列的锁互斥量-
odoLockstd::mutexodom 数据队列的锁互斥量-
subLaserCloudros::Subscriber订阅 "points_raw" Lidar 源数据-
pubLaserCloudros::Publisher这个玩意 没有用？？-
pubExtractedCloudros::Publisher发布 去畸变的点云数据 "lio_sam/deskew/cloud_deskewed" 从图2中看，貌似只给rviz 当调试数据用-
pubLaserCloudInforos::Publisher发布cloud_info :"lio_sam/deskew/cloud_info" 主要传递的数据结构，串联整个模块-
subImuros::Subscriber订阅 "imu_raw" IMU 源数据-
imuQueuestd::dequeIMU 源数据对应的队列-
subOdomros::Subscriber订阅"odometry/imu_incremental" 通过IMU IMU 预积分器 融合了Lidar里程计的当前时刻的里程计数据 same frequency as IMU std::deque odomQueue; // "odometry/imu_incremental" 对应的里程计的数据队列-
cloudQueuestd::dequelidar 点云的源数据队列-
currentCloudMsgsensor_msgs::PointCloud2从队列里取出来要处理的点云-
i// 保存一帧点云 帧内时间期间 的相对于帧内第一点的数据位姿旋转变换关系，根据IMU 数据频率采集计算得来-
double *imuTime = new double[queueLength]-
double *imuRotX = new double[queueLength]-
double *imuRotY = new double[queueLength]-
double *imuRotZ = new double[queueLength]-
imuPointerCurint遍历点云时用的 索引下标-
firstPointFlagbool标记点云中的第一个点，不需要作运动畸变矫正，并且计算到 transStartInverse-
transStartInverseEigen::Affine3f点云中每一线起始点的位姿变换矩阵的逆 乘以 某个点的变换矩阵 就是相对变换-
laserCloudInpcl::PointCloud::Ptr点云遍历时存放未运动畸变矫正的点云-
fullCloudpcl::PointCloud::Ptr处理完后，点云存放在这里，过滤盲区点、运动畸变矫正等-
extractedCloudpcl::PointCloud::Ptr与fullCloud 唯一的区别就是，fullCloud 严格按横列排序的，而extractedCloud 是紧凑无序的-
deskewFlagint是否作运动畸变矫正的标志，0 无需 1 要-
rangeMatcv::Mat深度图 最后赋值给cloudInfo.pointRange 传递出去-
里程计数据 求得 lidar 起始时刻 到末尾时刻的运动增量-
bool odomDeskewFlag-
float odomIncreX-
float odomIncreY-
float odomIncreZ-
cloudInfolio_sam::cloud_info自定义结构，这个结构要串联整个激光点云处理模块，重点关注-
timeScanCurdouble当帧点云起始时间戳-
timeScanEnddouble当帧点云末尾时间戳-
cloudHeaderstd_msgs::Headerros 消息头，主要是为了获取消息时间-
columnIdnCountVecvector这个数组主要是为了计算Livox激光雷达数据的横列用到的-

1.送入odomhandle，放入odomqueue中。

这里要说一下，注意两点：

第一，这个话题odometry/imu_incremental是imu里程计融合了激光里程计发送频率跟IMU 保持一致的里程计，是来自IMUPreintegration(imuPreintegration.cpp中的TransformFusion类)发布的里程计话题。它就是一个里程计数据，通过imu预积分计算优化来得到的任意时刻在世界坐标系下的位姿。

mapOptimization.cpp文件还会发布一个叫"lio_sam/mapping/odometry_incremental"，它代表的是激光里程计，这里的odometry/imu_incremental融合了激光里程计发送频率跟IMU 保持一致的里程计信息。

2.原始点云数据送入cloudhandle：

3.1 点云加入cloudQueue，检查数据有效性（格式上，主要因为雷达款式不同）

3.1.1 cachePointCloud

    • 保存Lidar 数据到队列中
    • 转换数据结构
    • 判断点云数据是否包含 ring 字段
    • 判断点云数据是否包含 time 字段
    • 设置点云开始、结束时间戳

3.2 deskewinfo： 分别收集IMU 数据和Odom中的在Lidar 数据帧内的期间内的位姿数据 3.2.1 imuDeskewInfo：遍历激光帧，从imuqueue中找当前激光帧前0.01s开始，当前帧后0.01s结束，找到最近的一个imu数据，把它的原始角度数据，作为cloudInfo.imuRollInit等变量。 这个函数主要是收集 Lidar 点云数据的开始时间到结束时间之间的IMU数据，用作点云运动畸变矫正用

    1. 提取lidar最早时刻的 IMU 的Roll Pitch Yaw 作为该点云数据的开始时刻的坐标方向
    1. 收集最开始时刻到Lidar 数据的最后时刻，相对于第一时刻(点云开始时刻)的旋转角
  • 其中第一个数据相对于第1个时刻的旋转角为0 然后往下递推
  • 旋转角 = 上一时刻旋转角 + 角速度 * 时间

3.2.2 odomDeskewInfo：遍历imu里程计的odomqueue队列，剔除当前0.01s之前的数据，找到第一个大于当前激光帧的数据，然后用里程计数据来初始化雷达，也是填充角度，不过这次填充的是cloudInfo.initialGuessRoll等变量，位置也会被填充到initialGuessX里。

    1. 通过里程计数据求得Lidar 数据帧内 起始时刻的位姿，并保存到cloud_info中，传给MapOptimization中
    1. 通过里程计数据求得Lidar 数据帧内 末尾时刻的位姿
    1. 通过1和2 求得 末尾时刻相对起始时刻的变换矩阵 ，并求得相关标量的增量

注意：cloudInfo.initialGuessRoll和cloudInfo.imuRollInit，这些东西都是角度数据，带Guess的为imu里程计提供的数据，imuRollInit这种为imu原始数据。如果有合适的，分别会填充cloudInfo.odomAvailable和cloudInfo.imuAvailable变量为true，代表这块的数据可用。这些会被用在mapOptimization.cpp的updateInitGuess函数，给激光里程计做一个初始化，然后在这个初始化的基础上进行非线性优化。

3.3 projectPointCloud：

3.3.1 获取点云中的每一个点，投影图像。

3.3.2 去畸变。假设一帧激光数据，是在一个很小的时间段内获取的，但是由于这段时间内，激光雷达可能处于运动状态，那么就可能导致运动畸变。这里就要用到了3.2.1中提到的imuRotX等玩意，根据imu的运动信息，把这段时间内依次接收的每个点统一投影到初始时刻，去除运动畸变。

3.4 CloudExtraction：

    1. 过滤一些盲区数据
    1. 跳线过滤压缩点云
    1. 计算点云中的点的列索引
    1. 点云运动畸变矫正
    1. 点云深度计算

pcl::cloudPoint2 里面保存的点云是一维的。 既然是一维数组，16线的点都弄到一维数组里，那就比较麻烦。虽然说这里要弄一个深度投影图像，但是其实进程之间传输的并不是这个深度投影图像，而是自定义的cloudinfo形式的数据。

实际上是想把有效的数据记录进来，例如空值不要，每根线前5个和后5个也不要……那这样16线的数据混到一维组里就分不清了，所以要记录下每根扫瞄线起始点在1维数组中的索引startRingIndex，结束点的索引endRingIndex，在原始图像中的列数pointColInd，距离值pointRange。然后把有效激光点，放到extractedCloud中。

3.5 publishClouds：

把extractedCloud发布出去，/lio_sam/deskew/cloud_deskewed，这个只有点云信息。

然后再把这些点，赋值到cloudInfo.cloud_deskewed中，把整个的cloudInfo发布出去，名字就叫“lio_sam/deskew/cloud_info"，这个就是作者自己定义的特殊的msg。

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
    {
        // 对IMU 数据做数据矫正，使得IMU 的坐标系方向与Lidar坐标系方向保持一致，这样IMU 与 Lidar 的外参仅差一个平移变换
        sensor_msgs::Imu thisImu = imuConverter(*imuMsg);

        std::lock_guard<std::mutex> lock1(imuLock);
        // IMU 的数据频率比较高，进来时先保留在队列中，然后等Lidar 数据进来时，按Lidar的数据周期处理数据
        imuQueue.push_back(thisImu);

        // debug IMU data
        // cout << std::setprecision(6);
        // cout << "IMU acc: " << endl;
        // cout << "x: " << thisImu.linear_acceleration.x << 
        //       ", y: " << thisImu.linear_acceleration.y << 
        //       ", z: " << thisImu.linear_acceleration.z << endl;
        // cout << "IMU gyro: " << endl;
        // cout << "x: " << thisImu.angular_velocity.x << 
        //       ", y: " << thisImu.angular_velocity.y << 
        //       ", z: " << thisImu.angular_velocity.z << endl;
        // double imuRoll, imuPitch, imuYaw;
        // tf::Quaternion orientation;
        // tf::quaternionMsgToTF(thisImu.orientation, orientation);
        // tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
        // cout << "IMU roll pitch yaw: " << endl;
        // cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl;
    }

    void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
    {
        std::lock_guard<std::mutex> lock2(odoLock);
        // 融合里程计的数据频率与IMU 的数据频率相同，进来时先保留在队列中，然后等Lidar 数据进来时，按Lidar的数据周期处理数据
        odomQueue.push_back(*odometryMsg);
    }

    /**
     * Lidar 数据的回调函数  主要逻辑都放在这里
     * */
    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
    {
        /** 这个函数1. 保存Lidar 数据到队列中
 *         2. 转换数据结构
 *         3. 判断点云数据是否包含 ring 字段
 *         4. 判断点云数据是否包含 time 字段
 *         5. 设置点云开始、结束时间戳 
 * */
        if (!cachePointCloud(laserCloudMsg))
            return;
/**
 * 分别收集IMU 数据和Odom中的在Lidar 数据帧内的期间内的位姿数据
 * */
        if (!deskewInfo())
            return;

        projectPointCloud();

        cloudExtraction();

        publishClouds();

        resetParameters();
    }
/** 这个函数1. 保存Lidar 数据到队列中
 *         2. 转换数据结构
 *         3. 判断点云数据是否包含 ring 字段
 *         4. 判断点云数据是否包含 time 字段
 *         5. 设置点云开始、结束时间戳 
 * */
    bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
    {
        // Lidar 数据存进队列末尾
        // cache point cloud
        cloudQueue.push_back(*laserCloudMsg);
        if (cloudQueue.size() <= 2)
            return false;
        // 取出最久的一帧点云数据
        // convert cloud
        currentCloudMsg = std::move(cloudQueue.front());
        // 取出之后就pop掉
        cloudQueue.pop_front();
        // 该系统支持 VELODYNE、LIVOX、OUSTER 三款激光雷达，但是OUSTER 数据需要转成VELODYNE 格式
        if (sensor == SensorType::VELODYNE || sensor == SensorType::LIVOX)
        {
            pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
        }
        else if (sensor == SensorType::OUSTER)
        {
            // Convert to Velodyne format
            pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);
            laserCloudIn->points.resize(tmpOusterCloudIn->size());
            laserCloudIn->is_dense = tmpOusterCloudIn->is_dense;
            for (size_t i = 0; i < tmpOusterCloudIn->size(); i++)
            {
                auto &src = tmpOusterCloudIn->points[i];
                auto &dst = laserCloudIn->points[i];
                dst.x = src.x;
                dst.y = src.y;
                dst.z = src.z;
                dst.intensity = src.intensity;
                dst.ring = src.ring;
                dst.time = src.t * 1e-9f;
            }
        }
        else
        {
            ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor));
            ros::shutdown();
        }

        // get timestamp
        cloudHeader = currentCloudMsg.header;
        // 当前帧的开始时间戳
        timeScanCur = cloudHeader.stamp.toSec();
        // 当前帧最后数据的时间戳
        timeScanEnd = timeScanCur + laserCloudIn->points.back().time;

        // check dense flag
        if (laserCloudIn->is_dense == false)
        {
            ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
            ros::shutdown();
        }

        // 检查数据是否有ring 字段，ring 表示该点云数据点所在的线数，没有标明ring 的点云数据 在该系统中失效
        // check ring channel
        static int ringFlag = 0;
        if (ringFlag == 0)
        {
            ringFlag = -1;
            for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
            {
                if (currentCloudMsg.fields[i].name == "ring")
                {
                    ringFlag = 1;
                    break;
                }
            }
            if (ringFlag == -1)
            {
                ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");
                ros::shutdown();
            }
        }
        // 检查点云数据是否有time 字段
        // check point time
        if (deskewFlag == 0)
        {
            deskewFlag = -1;
            for (auto &field : currentCloudMsg.fields)
            {
                if (field.name == "time" || field.name == "t")
                {
                    deskewFlag = 1;
                    break;
                }
            }
            if (deskewFlag == -1)
                ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");
        }

        return true;
    }
/**
 * 分别收集IMU 数据和Odom中的在Lidar 数据帧内的期间内的位姿数据
 * */
    bool deskewInfo()
    {
        std::lock_guard<std::mutex> lock1(imuLock);
        std::lock_guard<std::mutex> lock2(odoLock);

        // make sure IMU data available for the scan
        if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
        {
            ROS_DEBUG("Waiting for IMU data ...");
            return false;
        }
/***
 * 这个函数主要是收集 Lidar 点云数据的开始时间到结束时间之间的IMU数据，用作点云运动畸变矫正用
 * 1. 提取lidar最早时刻的 IMU 的Roll Pitch Yaw 作为该点云数据的开始时刻的坐标方向
 * 2. 收集最开始时刻到Lidar 数据的最后时刻，相对于第一时刻(点云开始时刻)的旋转角
 *    其中第一个数据相对于第1个时刻的旋转角为0  然后往下递推
 *    旋转角 = 上一时刻旋转角 + 角速度 * 时间
 * */
        imuDeskewInfo();
/**这个函数 1. 通过里程计数据求得Lidar 数据帧内 起始时刻的位姿，并保存到cloud_info中，传给MapOptimization中
 *         2. 通过里程计数据求得Lidar 数据帧内 末尾时刻的位姿
 *         3. 通过1和2 求得 末尾时刻相对起始时刻的变换矩阵 ，并求得相关标量的增量
 * */
        odomDeskewInfo();

        return true;
    }
/***
 * 这个函数主要是收集 Lidar 点云数据的开始时间到结束时间之间的IMU数据，用作点云运动畸变矫正用
 * 1. 提取lidar最早时刻的 IMU 的Roll Pitch Yaw 作为该点云数据的开始时刻的坐标方向
 * 2. 收集最开始时刻到Lidar 数据的最后时刻，相对于第一时刻(点云开始时刻)的旋转角
 *    其中第一个数据相对于第1个时刻的旋转角为0  然后往下递推
 *    旋转角 = 上一时刻旋转角 + 角速度 * 时间
 * */
    void imuDeskewInfo()
    {
        cloudInfo.imuAvailable = false;
        // pop 掉比 lidar 时间戳小0.01 的IMU数据
        while (!imuQueue.empty())
        {
            if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
                imuQueue.pop_front();
            else
                break;
        }

        if (imuQueue.empty())
            return;

        imuPointerCur = 0;

        for (int i = 0; i < (int)imuQueue.size(); ++i)
        {
            sensor_msgs::Imu thisImuMsg = imuQueue[i];
            double currentImuTime = thisImuMsg.header.stamp.toSec();

            // get roll, pitch, and yaw estimation for this scan
            if (currentImuTime <= timeScanCur)
            // 在9 轴 IMU 数据中提取 Roll pitch yaw 数据 
                imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);

            if (currentImuTime > timeScanEnd + 0.01)
                break;
            // 开始的那个时间点 用0 填充
            if (imuPointerCur == 0){
                imuRotX[0] = 0;
                imuRotY[0] = 0;
                imuRotZ[0] = 0;
                imuTime[0] = currentImuTime;
                ++imuPointerCur;
                continue;
            }
            // 提取IMU 数据的角速度
            // get angular velocity
            double angular_x, angular_y, angular_z;
            imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);
            // 然后递增的计算往后的IMU 的旋转角 
            // 旋转角 = 上一时刻旋转角 + 角速度 * 时间 
            // integrate rotation
            double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
            imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
            imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
            imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
            imuTime[imuPointerCur] = currentImuTime;
            ++imuPointerCur;
        }

        --imuPointerCur;

        if (imuPointerCur <= 0)
            return;

        cloudInfo.imuAvailable = true;  // IMU 旋转角可用
    }

/**这个函数 1. 通过里程计数据求得Lidar 数据帧内 起始时刻的位姿，并保存到cloud_info中，传给MapOptimization中
 *         2. 通过里程计数据求得Lidar 数据帧内 末尾时刻的位姿
 *         3. 通过1和2 求得 末尾时刻相对起始时刻的变换矩阵 ，并求得相关标量的增量
 * */
    void odomDeskewInfo()
    {
        cloudInfo.odomAvailable = false;

        while (!odomQueue.empty())
        {   // 去除 Lidar 时间开始时刻0.01秒以前的数据
            if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
                odomQueue.pop_front();
            else
                break;
        }

        if (odomQueue.empty())
            return;
        // 如果 里程计队列里没有比Lidar 时间更小的数据，返回，证明Odom用于点云运动畸变修正的数据不可用
        if (odomQueue.front().header.stamp.toSec() > timeScanCur)
            return;
        // startOdomMsg 时刻刚刚好大于或者等于 lidar 时刻，由于IMU Odom 的频率远高于Lidar 周期，startOdomMsg的时刻比较接近Lidar 的起始时刻
        // get start odometry at the beinning of the scan
        nav_msgs::Odometry startOdomMsg;

        for (int i = 0; i < (int)odomQueue.size(); ++i)
        {
            startOdomMsg = odomQueue[i];

            if (ROS_TIME(&startOdomMsg) < timeScanCur)
                continue;
            else
                break;
        }

        tf::Quaternion orientation;
        tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);

        double roll, pitch, yaw;
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
        // 对应着 Lidar 数据帧起始时刻的位姿
        // Initial guess used in mapOptimization
        cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x;
        cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y;
        cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z;
        cloudInfo.initialGuessRoll  = roll;
        cloudInfo.initialGuessPitch = pitch;
        cloudInfo.initialGuessYaw   = yaw;

        cloudInfo.odomAvailable = true;

        // get end odometry at the end of the scan
        odomDeskewFlag = false;

        if (odomQueue.back().header.stamp.toSec() < timeScanEnd)
            return;
        // endOdomMsg 获取 lidar 数据帧内的最后一时刻的位姿，有了最开始时刻的位姿和最后一时刻的位姿，就可以根据点云时间戳线性插值，修正点云运动畸变
        nav_msgs::Odometry endOdomMsg;

        for (int i = 0; i < (int)odomQueue.size(); ++i)
        {
            endOdomMsg = odomQueue[i];

            if (ROS_TIME(&endOdomMsg) < timeScanEnd)
                continue;
            else
                break;
        }

        if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))
            return;

        Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);

        tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation);
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
        Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);
        // 末尾时刻相对于起始时刻的变换矩阵
        Eigen::Affine3f transBt = transBegin.inverse() * transEnd;
        // 转换成 增量
        float rollIncre, pitchIncre, yawIncre;
        pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);

        odomDeskewFlag = true;
    }

/**
 * 根据激光点云的时间戳，在 imuRotX imuRotY imuRotZ 列表中按线形插值的方式寻找该时刻相对于点云起始时刻的旋转
 * */
    void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
    {
        *rotXCur = 0; *rotYCur = 0; *rotZCur = 0;

        int imuPointerFront = 0;
        while (imuPointerFront < imuPointerCur)
        {
            if (pointTime < imuTime[imuPointerFront])
                break;
            ++imuPointerFront;
        }

        if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
        {
            *rotXCur = imuRotX[imuPointerFront];
            *rotYCur = imuRotY[imuPointerFront];
            *rotZCur = imuRotZ[imuPointerFront];
        } else {
            int imuPointerBack = imuPointerFront - 1;
            double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
            double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
            *rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
            *rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
            *rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
        }
    }

    /**
 * 本来也是根据激光点云的时间戳按线性插值法找位移的，但是下面注释说了，如果系统运动的很慢，位移的运动畸变矫正意义不大，所以作者就没有做
 * */
    void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur)
    {
        *posXCur = 0; *posYCur = 0; *posZCur = 0;

        // If the sensor moves relatively slow, like walking speed, positional deskew seems to have little benefits. Thus code below is commented.

        // if (cloudInfo.odomAvailable == false || odomDeskewFlag == false)
        //     return;

        // float ratio = relTime / (timeScanEnd - timeScanCur);

        // *posXCur = ratio * odomIncreX;
        // *posYCur = ratio * odomIncreY;
        // *posZCur = ratio * odomIncreZ;
    }
/**
 * 对点云中的某个点进行运动畸变矫正，统一把点云映射到以点云起始时刻的位姿坐标系下
 * */
    PointType deskewPoint(PointType *point, double relTime)
    {
        if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
            return *point;

        double pointTime = timeScanCur + relTime;

        float rotXCur, rotYCur, rotZCur;
        findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);

        float posXCur, posYCur, posZCur;
        findPosition(relTime, &posXCur, &posYCur, &posZCur);

        if (firstPointFlag == true)
        {
            transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
            firstPointFlag = false;
        }

        // transform points to start
        Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
        Eigen::Affine3f transBt = transStartInverse * transFinal;

        PointType newPoint;
        // newpoint = transBt * [point|1]^T
        newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3);
        newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3);
        newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3);
        newPoint.intensity = point->intensity;

        return newPoint;
    }
/**
 * 1. 过滤一些盲区数据
 * 2. 跳线过滤压缩点云
 * 3. 计算点云中的点的列索引
 * 4. 点云运动畸变矫正
 * 5. 点云深度计算
 * */
    void projectPointCloud()
    {
        int cloudSize = laserCloudIn->points.size();
        // range image projection
        for (int i = 0; i < cloudSize; ++i)
        {
            PointType thisPoint;
            thisPoint.x = laserCloudIn->points[i].x;
            thisPoint.y = laserCloudIn->points[i].y;
            thisPoint.z = laserCloudIn->points[i].z;
            thisPoint.intensity = laserCloudIn->points[i].intensity;
            // 计算点云的距离，过滤掉属于盲区的点云
            float range = pointDistance(thisPoint);
            if (range < lidarMinRange || range > lidarMaxRange)
                continue;

            int rowIdn = laserCloudIn->points[i].ring;
            if (rowIdn < 0 || rowIdn >= N_SCAN)
                continue;
            // downsampleRate 这个字段是用来滤波的压缩数据，downsampleRate =2 时 128线 ->64线
            if (rowIdn % downsampleRate != 0)
                continue;

            int columnIdn = -1;
            if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER)
            {   // 计算该点的水平夹角，
                float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
                //Horizon_SCAN=1800,每格0.2度
                static float ang_res_x = 360.0/float(Horizon_SCAN);
            //horizonAngle 为[-180,180],horizonAngle -90 为[-270,90],-round 为[-90,270], /ang_res_x 为[-450,1350]
            //+Horizon_SCAN/2为[450,2250]
            // 即把horizonAngle从[-180,180]映射到[450,2250]
                columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;

            //大于1800，则减去1800，相当于把1801～2250映射到1～450
            //先把columnIdn从horizonAngle:(-PI,PI]转换到columnIdn:[H/4,5H/4],
            //然后判断columnIdn大小，把H到5H/4的部分切下来，补到0～H/4的部分。
            //将它的范围转换到了[0,H] (H:Horizon_SCAN)。
            //这样就把扫描开始的地方角度为0与角度为360的连在了一起，非常巧妙。
            //如果前方是x，左侧是y，那么正后方左边是180，右边是-180。
                if (columnIdn >= Horizon_SCAN)
                    columnIdn -= Horizon_SCAN;
            }
            else if (sensor == SensorType::LIVOX)
            {
                columnIdn = columnIdnCountVec[rowIdn];
                columnIdnCountVec[rowIdn] += 1;
            }
            
            if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
                continue;

            if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
                continue;

            thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);

            rangeMat.at<float>(rowIdn, columnIdn) = range;

            int index = columnIdn + rowIdn * Horizon_SCAN;
            fullCloud->points[index] = thisPoint;
        }
    }

    void cloudExtraction()
    {
        int count = 0;
        // extract segmented cloud for lidar odometry
        for (int i = 0; i < N_SCAN; ++i)
        {
            cloudInfo.startRingIndex[i] = count - 1 + 5;

            for (int j = 0; j < Horizon_SCAN; ++j)
            {
                if (rangeMat.at<float>(i,j) != FLT_MAX)
                {
                    // mark the points' column index for marking occlusion later
                    cloudInfo.pointColInd[count] = j;
                    // save range info
                    cloudInfo.pointRange[count] = rangeMat.at<float>(i,j);
                    // save extracted cloud
                    extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                    // size of extracted cloud
                    ++count;
                }
            }
            cloudInfo.endRingIndex[i] = count -1 - 5;
        }
    }
    
    void publishClouds()
    {
        cloudInfo.header = cloudHeader;
        cloudInfo.cloud_deskewed  = publishCloud(pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
        pubLaserCloudInfo.publish(cloudInfo);
    }

老虎福
关注
  • 1
    点赞
  • 踩
  • 8
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
LIO-SAM代码阅读详细注释版
11-16
本文将围绕LIO-SAM的代码进行详细解释，以帮助读者理解其核心思想和实现细节。 1. **系统框架** LIO-SAM采用EKF（扩展卡尔曼滤波）框架，结合IMU的连续运动模型和LiDAR的离散扫描数据，对传感器数据进行融合，以...
lio-sam-kitti
05-14
《LIO-SAM与KITTI数据集：激光雷达惯导集成SLAM的实践与解析》 在自动驾驶、机器人导航和地图构建等领域，定位与建图（Simultaneous Localization and Mapping, SLAM）技术扮演着至关重要的角色。LIO-SAM（LiDAR-...
ros学习（七）：sensor_msgs 消息类型
biter0088的博客
01-18 9383
一、sensor_msgs/Imu sensor_msgs/Imu Documentationhttp://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html # This is a message to hold data from an IMU (Inertial Measurement Unit) #这是一条保存来自IMU（惯性测量单元）的数据的消息 # Accelerations should be in m/s^2 (not in g's),
ROS消息类型之sensor_msgs::Imu
weixin_44884561的博客
11-06 2020
ROS消息类型之sensor_msgs::Imu
LIO-SAM源码解析（五）：mapOptmization.cpp
最新发布
人无远虑，必有近忧
05-21 1816
这个函数主要就是进行帧到地图的匹配，通过点到面、点到线的距离距离最小作为优化目标。LOAM中雅阁比矩阵推导其实还是过于复杂了，可以使用进行误差扰动来计算雅阁比矩阵，姿态使用失准角（李代数）模型推导更为简单。
ROS消息sensor_msgs::Imu数据格式
qq_35102059的博客
01-25 1万+
ubuntu下打开终端输入: rosmsg show sensor_msgs/Imu 查看sensor_msgs::Imu的数据格式: std_msgs/Header header uint32 seq time stamp // 时间戳 string frame_id geometry_msgs/Quaternion orientation // 姿态 float64 x float64 y float64 z float64 w float64[9
LIO-SAM前端点云去畸变源码-imageProjection.cpp解析
avenger_fang的博客
10-04 1737
以上两个畸变计算补偿量是独立的，独立计算IMU数据在当前处理帧(上一激光帧)的角度变化量，并存下来作为数值以备使用；同理里程计也是类似，不同的是，里程会记录下当前处理帧(上一激光帧)点云开始时刻的里程计的测量值，包括位置和姿态，其坐标量为odom topic的坐标系。遍历当前处理帧(最新回调进的一个帧的前一帧）点云的每一个点，每一个点都不错过，然后调用去畸变的函数deskewPoint(&thisPoint, laserCloudIn->points[i].time)去这一个点的畸变。
lio-sam学习笔记(三)
weixin_45572737的博客
02-09 748
lio-sam前端中关于特征提取和地图映射部分的学习。
LIO-SAM代码（中文注释）
02-24
**LIO-SAM（LiDAR-Inertial ...通过阅读和理解LIO-SAM的源代码，你可以深入学习如何在实际项目中实现LiDAR和IMU的融合，以及如何构建高效、精确的SLAM系统。这对于无人车、无人机等移动机器人平台的开发至关重要。
LIO-SAM:LIO-SAM
03-18
LIO-SAM实时激光雷达惯性测距软件包。我们强烈建议用户彻底阅读本文档，并首先使用提供的数据集对软件包进行测试。有关该方法演示的视频可在找到。菜单（必须读取）（必须读取）系统架构我们设计了一个系统，该系统...
LIO-SAM代码逻辑
04-22
LIO-SAM代码逻辑 LIO-SAM代码逻辑是基于激光雷达 Odometry 和惯导测量单元（Inertial Measurement Unit，IMU）数据的SLAM算法，其核心逻辑包括前端雷达点云处理、imu数据处理、点云补偿、特征点云提取、运动补偿和...
对IMU数据进行卡尔曼滤波
qq_53576285的博客
03-19 3424
我们要使用IMU数据，必须对数据进行预处理，卡尔曼滤波就是很好的方式。有c++的卡尔曼滤波代码实现还有利用MATLAB自带模块进行滤波。
【cartographer without ros】五、陀螺仪Imu数据转换
Cloud_H的博客
08-08 1201
本节将介绍陀螺仪Imu数据转换为cartographer中的数据。还有cartographer中的ImuData数据和Ros的数据相互转换，方便引入Rosbag进行储存和读取。
【SLAM学习】获取IMU和雷达消息并发布
Q小鑫
07-03 555
本文主要记录如何将rosbag的消息进行获取并进行发布以及后续处理。
lio-sam运行思路整理——预积分模块imuHandler()
hht12138的博客
08-22 910
这里是个人对于lio-sam运行流程的一些理解，如有不对之处，欢迎批评指正，共同进步
基于ROS与C++，只利用IMU实现里程计和计步器，精确计算行走距离
weixin_43890835的博客
05-24 3925
只利用IMU单个传感器，实现对人行走距离的检测，精度达到0.5m左右。同时还利用峰值检测算法实现了一个精确的计步器。
【SLAM】LIO-SAM解析——数据预处理imageProjection(2)
iwanderu的博客
02-21 5493
这部分内容对应imageProjection.cpp，雷达帧是一定角度旋转的(例如360°)，同时机器人也在运动，这就造成了雷达在不同角度获取数据时机器人的位姿是不同的，也就是说，lidar在一帧时间范围内的每一个时刻所在的坐标系是不同的，对应获得的点云也是在不同坐标系下的。 这部分代码的作用就是把雷达旋转一周时，所有的点云都转换到同一个位姿下，也就是该帧初始时刻的lidar坐标系下。如何做到呢？初始时刻的位姿我知道，起止时间内的位姿变化我知道，每一个点云的时刻我知道，那么每一个时刻相对于初始时刻的位姿变化
ROS点云类型sensor_msgs::PointCloud2与PCL的PointCloud＜T＞点云类型转换
热门推荐
木卫三的博客
11-03 1万+
ROS中sensor_msgs::PointCloud2类型定义；与PCL的PointCloud点云数据类型转换；moveFromROSMsg()函数解析；点云格式转换中的注意事项。
九轴IMU和sensor_msgs::Imu消息介绍
ergevv的博客
04-11 1161
九轴IMU、sensor_msgs::Imu
lio-sam代码阅读
11-17
lio-sam是一个开源项目，是LIO(Linux内核iSCSI target)模块的一个分支。它是专门为高性能和可扩展性而设计的iSCSI目标代码。 lio-sam项目的主要目标是提供一个高性能的iSCSI目标，同时保持Linux kernel的稳定性和可靠性。它在传输层使用Scst（SCSI target实现）和LIO（Linux iSCSI实现）的组合，并有一些优化以提高性能。它还支持各种iSCSI功能，如CHAP认证、数据压缩和IPsec等。 代码阅读lio-sam对Linux内核和iSCSI有一定的了解是很有帮助的。lio-sam使用了一些Linux内核的机制，如工作队列和内存管理。了解这些机制将有助于理解lio-sam的实现原理和性能优化技巧。 在阅读lio-sam代码时，可以关注以下几个方面： 1. LIO模块的初始化和配置：lio-sam在加载模块时进行一些初始化工作，包括创建Scst的实例和配置iSCSI target。了解这些步骤可以帮助理解lio-sam的工作流程和配置方式。 2. iSCSI连接管理：lio-sam负责管理iSCSI连接，包括连接的建立、维护和中断。了解连接管理的实现原理可以帮助理解lio-sam如何处理多个客户端的连接和请求。 3. SCSI命令处理：lio-sam的核心功能是处理SCSI命令。了解lio-sam如何解析SCSI命令、调用底层存储设备和返回响应可以帮助理解其工作原理和性能优化方法。 4. 性能优化技巧：lio-sam的设计目标之一是提高性能。代码中可能包含一些性能优化技巧，如批量处理、IO调度和缓存管理等。了解这些技巧可以帮助优化自己的应用程序。 需要注意的是，代码阅读是一项耗时耗力的工作，需要具备一定的编程和系统知识。在阅读代码时，可以结合官方文档、论坛和社区来获取更多的信息和帮助。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值