        Point cloud upsampling, also known as point cloud densification, aims to take a sparse point cloud as input and generate a dense point cloud as output. Among the many SLAM (Simultaneous Localization and Mapping, simultaneous positioning and map construction) algorithms, many algorithms are designed for autonomous driving or embedded in edge computing devices (such as TX2, STM32).

Due to the critical computational time complexity required in these contexts, researchers typically process sensor-generated point clouds. For example, for visual SLAM-based algorithms, point clouds are usually generated by means of depth maps, binocular odometry, or triangulation. To control computational complexity, the researchers downsample these point clouds to reduce the number of points. However, in some specific scenarios, it is necessary to obtain dense point cloud data, such as in applications such as ancient building scanning, forest mapping, and laser point cloud coloring.

        In the field of SLAM, the methods of point cloud upsampling can be divided into the following categories:

        1. ** Disable the code part of point cloud downsampling ** (Although this will increase the computational complexity, it will also improve the system accuracy due to the increase in the number of nonlinear optimization iterations).

2. Upsample the sparse point cloud by         ** linear interpolation ** (although it will affect the accuracy of the SLAM system, it can make the point cloud appear smoother).

        3. ** Improve sensor performance ** (by investing more money in higher resolution devices).

        4. ** Nonlinear interpolation based on neighborhood information **: Use the neighboring point information of each point in the point cloud to perform more complex interpolation to generate a more realistic dense point cloud. This method considers the relationship between points and can improve the point cloud density while maintaining accuracy.

        5. **Deep Learning Based Generative Model** : Generate dense point clouds from sparse point clouds using deep learning techniques such as generative adversarial networks (GAN) or variational autoencoders (VAE). This method can generate more detailed point cloud data in some scenarios.

        6. ** Multi-sensor fusion **: Combine data from multiple sensors, such as cameras and lidars, and use their complementary advantages for point cloud fusion. This can improve the density and accuracy of point clouds, especially in multi-sensor SLAM systems.

        In this article, we take LIO-SAM as an example to explain the first method.

2 Take LIO-SAM as an example to explain method 1

2.1 IMU pre-integration code analysis + point cloud encryption idea

The flow chart of the IMU pre-integration part in LIO-SAM is as follows:

Let's first look at the main function
int main(int argc, char** argv)
{
    ros::init(argc, argv, "roboat_loam");
    
    IMUPreintegration ImuP;

    TransformFusion TF;

    ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");

    // 分配4个线程执行这个节点
    ros::MultiThreadedSpinner spinner(4);

    // 程序阻塞，后台等待回调函数的执行
    spinner.spin();
    
    return 0;
}
1. `int main(int argc, char** argv)`: This is the entry function of the C++ program, which accepts the command line parameters `argc` and `argv`.

2. `ros::init(argc, argv, "roboat_loam")`: This line of code initializes the ROS node, uses the parameters `argc` and `argv`, and specifies the name of the node as "roboat_loam" .

3. `IMUPreintegration ImuP;`: This line of code creates an `IMUPreintegration` object named `ImuP`. It is an instantiated object of a custom class representing the function of performing IMU pre-integration.

4. `TransformFusion TF;`: This line of code creates a `TransformFusion` object named `TF`. It is an instantiated object of a custom class, representing the function of coordinate transformation fusion.

5. `ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m")`: This line of code outputs a ROS information log showing "IMU Preintegration Started". A special character escape sequence is used to color the output text to green.

6. `ros::MultiThreadedSpinner spinner(4)`: This line of code creates a `MultiThreadedSpinner` object, which is used to execute the callback function of the multi-threaded node. If the number of threads is specified as 4, it means that 4 threads are used to execute the callback function.

7. `spinner.spin()`: This line of code starts the execution of the multi-threaded callback function, and blocks the program, making it wait for the execution of the callback function.

8. `return 0;`: This line of code indicates that the program ends normally and returns 0.

To sum up, this code initializes the ROS node, creates `IMUPreintegration` and `TransformFusion` objects, and outputs a ROS information log. Then, the callback function of the node is executed in a multi-threaded manner, until all the callback functions are executed, the program ends and returns 0.

2.1.1 Callback function for receiving IMU information

    void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
        std::lock_guard<std::mutex> lock(mtx);

        // ROS_TIME函数用于获取当前odomMsg的时间戳的值
        double currentCorrectionTime = ROS_TIME(odomMsg);

        // make sure we have imu data to integrate
        // 判断IMU队列是否有数据
        if (imuQueOpt.empty())
            return;

        // 获取里程计 平移 XYZ 旋转xyzw
        float p_x = odomMsg->pose.pose.position.x;
        float p_y = odomMsg->pose.pose.position.y;
        float p_z = odomMsg->pose.pose.position.z;
        float r_x = odomMsg->pose.pose.orientation.x;
        float r_y = odomMsg->pose.pose.orientation.y;
        float r_z = odomMsg->pose.pose.orientation.z;
        float r_w = odomMsg->pose.pose.orientation.w;

        // 有退化风险（里程计的精度有风险下降）
        bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;

        // 位姿转化成gtsam格式（旋转 + 平移）
        gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));


        // 0. 初始化
        if (systemInitialized == false)
        {
            // 将优化问题复位
            resetOptimization();

            // 把IMU数据扔掉
            while (!imuQueOpt.empty())
            {
                // 如果IMU数据的时间戳 < 当前里程计的时间戳
                if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
                {
                    // 被扔掉IMU数据的时间戳，这个变量最终保存刚好小于当前里程计时间戳的帧
                    lastImuT_opt = ROS_TIME(&imuQueOpt.front());
                    imuQueOpt.pop_front();
                }
                else
                    break;
            }

             添加约束

            // 将LIDAR的位姿转换到IMU坐标系下
            prevPose_ = lidarPose.compose(lidar2Imu);
            gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
            graphFactors.add(priorPose);

            // gtsam::Vector3表示三自由度速度，由于刚开始确实不知道是多少，直接置为0了
            prevVel_ = gtsam::Vector3(0, 0, 0);
            // 设置其初始值和置信度
            gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
            // 约束加入到因子中
            graphFactors.add(priorVel);



            // gtsam::imuBias::ConstantBias表示IMU零偏   X(Pose3 xyzrpy)  Y(Vel xyz) B(BIAS axayazgxgygz)
            prevBias_ = gtsam::imuBias::ConstantBias();
            // 这段代码是使用GTSAM库中的PriorFactor类创建一个先验因子（PriorFactor），用于对IMU偏差（imuBias::ConstantBias）进行约束。
            //具体解释如下：
            // gtsam::PriorFactor<gtsam::imuBias::ConstantBias>：这是一个GTSAM库中的PriorFactor类的模板实例化，指定了模板参数为gtsam::imuBias::ConstantBias，即IMU偏差的类型。
            // priorBias：这是创建的先验因子对象的名称。
            // B(0)：这是一个表示IMU偏差的变量，其中(0)是索引号，用于区分多个偏差变量。这里使用索引号为0的偏差变量。
            // prevBias_：这是一个先前的IMU偏差的变量，它被用作先验因子的先验值。
            // priorBiasNoise：这是一个表示偏差噪声的对象，用于指定对IMU偏差的不确定性。
            // 综合起来，这段代码创建了一个先验因子，它约束了IMU偏差变量B(0)的值必须接近于先前的偏差值prevBias_，并且在计算图优化过程中使用了指定的偏差噪声来表示对这个约束的不确定性。
            gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
            // 将约束添加到因子中
            graphFactors.add(priorBias);


             添加状态

            // 添加状态量
            graphValues.insert(X(0), prevPose_);
            graphValues.insert(V(0), prevVel_);
            graphValues.insert(B(0), prevBias_);

             约束graphFactors + 状态 graphValues
            // 约束和状态添加完了，开始优化
            optimizer.update(graphFactors, graphValues);
            // 清空变量 方便下一次进行优化
            graphFactors.resize(0);
            graphValues.clear();

            // 预积分的接口，使用初始零值进行初始化
            imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
            imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
            
            key = 1;
            // 初始化标志位置为true
            systemInitialized = true;
            return;
        }


        // reset graph for speed
        // 执行100次 当isam优化器中加入较多的约束后，为了避免运算时间变长，直接清空 初始化因子图优化问题
        if (key == 100)
        {
            // get updated noise before reset
            // 取出最新时刻位姿 速度 零偏的协方差矩阵
            gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
            gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise  = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
            gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
            // reset graph
            // 复位整个优化问题
            resetOptimization();
            // add pose
            gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
            graphFactors.add(priorPose);
            // add velocity
            gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
            graphFactors.add(priorVel);
            // add bias
            gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
            graphFactors.add(priorBias);
            // add values
            graphValues.insert(X(0), prevPose_);
            graphValues.insert(V(0), prevVel_);
            graphValues.insert(B(0), prevBias_);
            // optimize once
            optimizer.update(graphFactors, graphValues);
            graphFactors.resize(0);
            graphValues.clear();

            key = 1;
        }


        // 1. 初始化结束后，我们已经有一个LIDAR坐标系转换到IMU坐标系下的一个位姿，下面开始形成预积分的约束
        while (!imuQueOpt.empty())
        {
            // pop and integrate imu data that is between two optimizations
            sensor_msgs::Imu *thisImu = &imuQueOpt.front();
            double imuTime = ROS_TIME(thisImu);
            // IMU的时间戳要小于LIDAR的时间戳
            if (imuTime < currentCorrectionTime - delta_t)
            {
                // 计算两个IMU之间的时间戳
                double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
                // 调用预积分接口将IMU数据送进去处理
                imuIntegratorOpt_->integrateMeasurement(
                        gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
                        gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);
                
                lastImuT_opt = imuTime;
                imuQueOpt.pop_front();
            }
            else
                break;
        }

        // 两帧间IMU预积分完成(LIDAR两帧，IMU很多帧)后，将其转换成预积分约束
        const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);

        // 预积分的约束对相邻两帧之间的位姿 速度 零偏形成约束
        // 上一帧的位姿、速度 当前帧的位姿、速度 上一帧的零偏
        gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
        // 约束加入因子图
        graphFactors.add(imu_factor);

        // 这段代码是使用GTSAM库中的BetweenFactor类创建一个因子（BetweenFactor），用于表示两个IMU偏差变量之间的关系。
        // graphFactors：这是一个图（graph）对象，用于存储因子（factor）。
        // gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>：这是一个GTSAM库中的BetweenFactor类的模板实例化，指定了模板参数为gtsam::imuBias::ConstantBias，即IMU偏差的类型。
        // B(key - 1)：这是表示先前的IMU偏差变量的变量，其中key - 1是索引号，用于区分多个偏差变量。这里使用索引号为key - 1的偏差变量。
        // B(key)：这是表示当前的IMU偏差变量的变量，其中key是索引号，用于区分多个偏差变量。这里使用索引号为key的偏差变量。
        // gtsam::imuBias::ConstantBias()：这是一个默认构造的IMU偏差对象，作为两个偏差变量之间的初始估计。
        // gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)：这是一个表示噪声模型的对象，用于表示两个偏差变量之间的不确定性。imuIntegratorOpt_->deltaTij()表示IMU积分器在两个时间步之间的时间差，noiseModelBetweenBias是一个噪声模型，通过对两个时间步之间的时间差进行标准化来定义偏差之间的不确定性。
        // 综合起来，这段代码创建了一个因子，它表示了两个IMU偏差变量之间的关系，并使用指定的噪声模型来表示这个关系的不确定性。然后，该因子被添加到一个图对象中，以便在后续的图优化过程中使用。
        graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
                         gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));

        // 这段代码将激光雷达位姿(lidarPose)与激光雷达坐标系到IMU坐标系之间的变换关系(lidar2Imu)进行组合操作，得到了当前的位姿(curPose)。
        // 这通常用于将激光雷达位姿转换到IMU坐标系下的位姿
        gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
        // LIDAR位姿补偿到IMU坐标系下同时根据是否退化选择不同的置信度作为这一帧的先验估计
        gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
        // 加入到因子图
        graphFactors.add(pose_factor);

        // 根据上一时刻的状态 结合预积分的结果 对当前状态进行预测  prevState_上一关键帧的位姿 + prevBias_上一关键帧的零偏
        // 这段代码使用GTSAM库中的ImuIntegrator类的predict()函数，基于先前的状态和偏差预测生成一个新的导航状态。
        // gtsam::NavState propState_：这是一个NavState类型的对象，用于存储生成的预测位姿
        // imuIntegratorOpt_：这是一个ImuIntegrator类的指针或引用，用于执行IMU积分和状态预测操作。
        // prevState_：这是一个先前的导航状态对象，作为IMU积分的起始状态。
        // prevBias_：这是一个先前的IMU偏差对象，用作IMU积分的起始偏差。
        // predict(prevState_, prevBias_)：这是ImuIntegrator类的成员函数，根据先前的状态和偏差，通过IMU积分预测生成一个新的位姿状态。
        // 综合起来，这段代码使用ImuIntegrator类的predict()函数，基于先前的导航状态和偏差，进行IMU积分预测，生成一个新的导航状态，并将其存储在propState_中。
        // 这个预测的导航状态通常用于在导航和状态估计问题中进行运动预测和滤波更新。
        gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);

        // 预测量作为初始值插入到因子图中
        graphValues.insert(X(key), propState_.pose());
        graphValues.insert(V(key), propState_.v());
        graphValues.insert(B(key), prevBias_);

        // 执行因子图优化
        optimizer.update(graphFactors, graphValues);
        optimizer.update();

        // 清空
        graphFactors.resize(0);
        graphValues.clear();

        // 获取优化结果
        gtsam::Values result = optimizer.calculateEstimate();

        // 获取优化后的当前状态作为当前帧的最佳估计
        prevPose_  = result.at<gtsam::Pose3>(X(key));
        prevVel_   = result.at<gtsam::Vector3>(V(key));
        prevState_ = gtsam::NavState(prevPose_, prevVel_);
        prevBias_  = result.at<gtsam::imuBias::ConstantBias>(B(key));

        // 当前约束任务已完成，预积分约束复位，同时需要设置一下零偏作为下一次积分的先诀条件
        imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
        // check optimization
        // 一个简单的失败检测
        if (failureDetection(prevVel_, prevBias_))
        {
            // 状态异常直接复位
            resetParams();
            return;
        }


        // 2. 优化之后，根据最新的IMU状态进行传播
        // 因为我们“同时”获得IMU和LIDAR里程计位姿，通常IMU数据会来的更早一点，LIDAR数据并不是原始数据而是一个里程计的数据（比如我们10s只能拿到第9s的odom的数据）
        // 而IMU数据是实时得到的（系统时间100s 可以收到99s的IMU 只能收到90s的odom），然而我们需要实时性，利用余下的IMU推算位姿
        prevStateOdom = prevState_;
        prevBiasOdom  = prevBias_;
        // first pop imu message older than current correction data
        double lastImuQT = -1;

        // 首先把LIDAR帧之前的IMU数据全部弹出去（和之前的IMU队列不是一个，需要注意）
        while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
        {
            lastImuQT = ROS_TIME(&imuQueImu.front());
            imuQueImu.pop_front();
        }

        // 如果有新于LIAR状态的IMU数据
        if (!imuQueImu.empty())
        {
            // 将这个预积分变量复位
            imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);

            for (int i = 0; i < (int)imuQueImu.size(); ++i)
            {
                sensor_msgs::Imu *thisImu = &imuQueImu[i];
                double imuTime = ROS_TIME(thisImu);
                double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);

                imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
                                                        gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);
                lastImuQT = imuTime;
            }
        }

        // 每做一次优化 ++key 到key==100 执行前段代码
        ++key;
        doneFirstOpt = true;
    }

The comments are already very detailed, let's explain the steps:

1. Check whether there is data in the IMU queue imuQueOpt , and take out the data of the odometer. And convert the pose of odom into gtsam format.

2. If the system is not initialized ( systemInitialized == false ): set the optimization problem (initialize graphFactors, graphValues ), and discard the data whose imuQueOpt is smaller than the current odometer data timestamp.

Calculate the initial value:
LIDAR pose: convert the incoming ODOM coordinates to the LIDAR coordinate system as the initial value

Speed: 0

IMU bias: setting

Initialize the pre-integrated interface ( imuIntegratorImu_, imuIntegratorOpt_ ) with an initial zero value and initialize the bias, and set the key to 1 (the first optimization). Set the initial bit to 1.

3. After the initialization, we already have a LIDAR coordinate system converted to a pose under the IMU coordinate system, and the pre-integration constraints begin to be formed below.

using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)

Take out one frame of data in imuQueOpt (IMU data frame between two frames of ODOM).

imuIntegratorOpt_Compute pre-integration.

const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);

Store the preintegration result between two frames into preint_imu.

Add constraints: By creating a `gtsam::ImuFactor` object, add the constraints between the pose, velocity and zero bias between two adjacent frames to the factor graph. This factor uses the pose and velocity of the previous frame (`X(key - 1)` and `V(key - 1)`), the pose and velocity of the current frame (`X(key)` and `V( key)`), and the zero bias of the previous frame (`B(key - 1)`) as input. At the same time, the result of the preintegration (`preint_imu`) is also used to define the constraints.

        // 两帧间IMU预积分完成(LIDAR两帧，IMU很多帧)后，将其转换成预积分约束
        const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);

        // 预积分的约束对相邻两帧之间的位姿 速度 零偏形成约束
        // 上一帧的位姿、速度 当前帧的位姿、速度 上一帧的零偏
        gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
        // 约束加入因子图
        graphFactors.add(imu_factor);

Take out the pre-integration result as the constraint initial value, and perform factor graph optimization.

4. Update the best optimization value of the current frame

        prevPose_  = result.at<gtsam::Pose3>(X(key));
        prevVel_   = result.at<gtsam::Vector3>(V(key));
        prevState_ = gtsam::NavState(prevPose_, prevVel_);
        prevBias_  = result.at<gtsam::imuBias::ConstantBias>(B(key));

5. The current constraint task has been completed, and the pre-integration constraint is reset. At the same time, it is necessary to set the zero offset as the prerequisite for the next integration.

imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);

6. Perform failure detection: use the speed and IMU bias to make a simple judgment. If it fails, reset the system.

    bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur)
    {
        Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
        // 当前速度超过30m/s 108km/h认为是异常状态
        if (vel.norm() > 30)
        {
            ROS_WARN("Large velocity, reset IMU-preintegration!");
            return true;
        }

        Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());
        Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());
        // 零偏太大，那也不太正常
        if (ba.norm() > 1.0 || bg.norm() > 1.0)
        {
            ROS_WARN("Large bias, reset IMU-preintegration!");
            return true;
        }

        return false;
    }

    void resetParams()
    {
        lastImuT_imu = -1;
        doneFirstOpt = false;
        systemInitialized = false;
    }

7. After optimization, propagate according to the latest IMU status, because we "simultaneously" obtain the pose of the IMU and LIDAR odometer, usually the IMU data will come earlier, and the LIDAR data is not the original data but the data of an odometer (For example, we can only get the odom data of the 9th s in 10s) and the IMU data is obtained in real time (the system time of 100s can receive the IMU of 99s, but the odom of 90s can only be received), but we need real-time performance and use the remaining The IMU estimates the pose.

        prevStateOdom = prevState_;
        prevBiasOdom  = prevBias_;
        // first pop imu message older than current correction data
        double lastImuQT = -1;

        // 首先把LIDAR帧之前的IMU数据全部弹出去（和之前的IMU队列不是一个，需要注意）
        while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
        {
            lastImuQT = ROS_TIME(&imuQueImu.front());
            imuQueImu.pop_front();
        }

        // 如果有新于LIAR状态的IMU数据
        if (!imuQueImu.empty())
        {
            // 将这个预积分变量复位
            imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);

            for (int i = 0; i < (int)imuQueImu.size(); ++i)
            {
                sensor_msgs::Imu *thisImu = &imuQueImu[i];
                double imuTime = ROS_TIME(thisImu);
                double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);

                imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
                                                        gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);
                lastImuQT = imuTime;
            }
        }

        // 每做一次优化 ++key 到key==100 执行前段代码
        ++key;
        doneFirstOpt = true;

8. Execute 100 times. When more constraints are added to the isam optimizer, in order to avoid longer calculation time, the initialization factor graph optimization problem is directly cleared.

        if (key == 100)
        {
            // get updated noise before reset
            // 取出最

Speaking of point cloud upsampling in SLAM

1 What is point cloud upsampling and why point cloud upsampling is needed

2 Take LIO-SAM as an example to explain method 1

2.1 IMU pre-integration code analysis + point cloud encryption idea

2.1.1 Callback function for receiving IMU information

Guess you like