VINS_Fusion部分代码讲解

已于 2024-12-12 21:10:17 修改
阅读量1.1k 收藏 20
点赞数 23
分类专栏： SLAM 立体视觉 文章标签： Vins_Fusion_gpu Jetson Orin NX VINS_Fusion
于 2024-12-12 16:10:04 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_42759162/article/details/144427952
版权

文章目录

  • VINS_Fusion部分代码讲解
    • estimator.cpp
      • Estimator::getIMUInterval
      • Estimator::processIMU
      • Estimator::processImage
        • 流程和功能总结
      • Estimator::processMeasurements
        • 流程和功能总结

VINS_Fusion部分代码讲解


图片来源：VINS_Fusion 框架

estimator.cpp

Estimator::getIMUInterval

这段代码是一个名为 Estimator::getIMUInterval 的成员函数，用于从两个缓冲区（accBuf 和 gyrBuf）中提取一定时间范围dt内的加速度计（IMU 加速度数据）和陀螺仪（IMU 角速度数据）。具体来说，该函数执行以下操作：

  1. 检查缓冲区是否为空：如果 accBuf 为空，则打印一条消息 “not receive imu” 并返回 false。
  2. 验证时间范围的有效性：如果请求的时间范围 [t0, t1] 在 accBuf 缓冲区的最后时间之前或等于它，则继续处理；否则，打印 “wait for imu” 并返回 false。
  3. 移除过时的数据：通过循环移除 accBuf 和 gyrBuf 中所有时间戳小于 t0 的数据对。
  4. 提取所需的数据：通过循环将 accBuf 和 gyrBuf 中时间戳在 [t0, t1) 范围内的数据对添加到 accVector 和 gyrVector 中，并从缓冲区中移除这些数据。
  5. 添加最后一个数据点：将 accBuf 和 gyrBuf 中第一个剩余的数据对（即刚好大于或等于 t1 的数据对）也添加到 accVector 和 gyrVector 中。
  6. 成功返回：完成上述步骤后，返回 true 表示成功获取了所需的 IMU 数据。

以下是代码的详细注释版本：

bool Estimator::getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector, 
                                vector<pair<double, Eigen::Vector3d>> &gyrVector)
{
    // 检查加速度计缓冲区是否为空
    if(accBuf.empty())
    {
        printf("not receive imu\n"); // 打印错误信息
        return false; // 返回失败状态
    }

    // 检查请求的时间范围是否有效
    if(t1 <= accBuf.back().first)
    {
        // 移除所有时间戳小于 t0 的数据对
        while (accBuf.front().first <= t0)
        {
            accBuf.pop();
            gyrBuf.pop();
        }
        
        // 提取时间戳在 [t0, t1) 范围内的数据对
        while (accBuf.front().first < t1)
        {
            accVector.push_back(accBuf.front()); // 添加加速度计数据对到结果向量
            accBuf.pop(); // 从缓冲区移除已处理的数据对
            gyrVector.push_back(gyrBuf.front()); // 添加陀螺仪数据对到结果向量
            gyrBuf.pop(); // 从缓冲区移除已处理的数据对
        }
        
        // 添加最后一个数据点（刚好大于或等于 t1 的数据对）
        accVector.push_back(accBuf.front());
        gyrVector.push_back(gyrBuf.front());
    }
    else
    {
        printf("wait for imu\n"); // 打印等待信息
        return false; // 返回失败状态
    }
    
    return true; // 返回成功状态
}

这个函数的主要目的是为了确保从 IMU 获取的数据是连续且准确的，在特定时间段内进行数据采集。

Estimator::processIMU

这段代码是 Estimator::processIMU 函数，用于处理 IMU（惯性测量单元）数据。具体来说，该函数负责初始化 IMU 数据、更新预积分器、存储传感器数据，并根据这些数据更新姿态、位置和速度估计。以下是详细的解释：

  1. 初始化 IMU 数据：

    • 如果这是第一次接收到 IMU 数据 (first_imu 为 false)，则设置 acc_0 和 gyr_0 为当前的加速度计和陀螺仪读数，并将 first_imu 设置为 true。

  2. 创建或获取预积分器：

    • 如果当前帧的预积分器 (pre_integrations[frame_count]) 尚未创建，则创建一个新的 IntegrationBase 实例，并使用初始加速度计和陀螺仪偏置 (Bas[frame_count] 和 Bgs[frame_count]) 进行初始化。

  3. 处理非第一帧的数据：

    • 如果不是第一帧 (frame_count != 0)，则将当前的时间步长 dt、线性加速度 linear_acceleration 和角速度 angular_velocity 推入到当前帧的预积分器中。
    • 同时，将 dt、linear_acceleration 和 angular_velocity 存储到相应的缓冲区 (dt_buf、linear_acceleration_buf 和 angular_velocity_buf) 中。
    • 计算未经校准的加速度 (un_acc_0 和 un_acc_1) 和角速度 (un_gyr)。
    • 更新旋转矩阵 Rs[j]，表示当前的姿态。
    • 根据未经校准的加速度计算新的位置 Ps[j] 和速度 Vs[j]。

  4. 更新上一时刻的 IMU 数据：

    • 最后，将当前的加速度计和陀螺仪读数分别赋值给 acc_0 和 gyr_0，以便在下一帧中使用。

以下是代码的详细注释版本：

void Estimator::processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
    // 如果这是第一次接收到 IMU 数据
    if (!first_imu)
    {
        first_imu = true; // 设置标志位为已接收首次 IMU 数据
        acc_0 = linear_acceleration; // 初始化初始加速度
        gyr_0 = angular_velocity; // 初始化初始角速度
    }

    // 创建或获取当前帧的预积分器
    if (!pre_integrations[frame_count])
    {
        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    }

    // 处理非第一帧的数据
    if (frame_count != 0)
    {
        // 将当前时间步长、线性加速度和角速度推入到预积分器
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        //if(solver_flag != NON_LINEAR)
            tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);

        // 存储当前时间步长、线性加速度和角速度到缓冲区
        dt_buf[frame_count].push_back(dt);
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);
        angular_velocity_buf[frame_count].push_back(angular_velocity);

        int j = frame_count;

        // 计算未经校准的加速度和角速度
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];

        // 更新旋转矩阵，表示当前的姿态
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();

        // 计算未经校准的加速度
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);

        // 更新位置和速度
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
        Vs[j] += dt * un_acc;
    }

    // 更新上一时刻的 IMU 数据
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity; 
}

这个函数的主要目的是确保 IMU 数据被正确地处理和集成，以便进行后续的状态估计（如姿态、位置和速度）。

Estimator::processImage

下面是对 Estimator::processImage 函数的详细逐行解释：

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const double header)
{
    // 打印调试信息，表示新图像到来
    ROS_DEBUG("new image coming ------------------------------------------");
    // 打印调试信息，表示正在添加特征点
    ROS_DEBUG("Adding feature points %lu", image.size());

    // 检查是否可以通过视差添加特征点
    if (f_manager.addFeatureCheckParallax(frame_count, image, td))
    {
        // 如果可以添加特征点，则设置边缘化标志为 MARGIN_OLD，表示这是一个关键帧
        marginalization_flag = MARGIN_OLD;
        //printf("keyframe\n");
    }
    else
    {
        // 否则，设置边缘化标志为 MARGIN_SECOND_NEW，表示这不是一个关键帧
        marginalization_flag = MARGIN_SECOND_NEW;
        //printf("non-keyframe\n");
    }

    // 根据边缘化标志打印调试信息
    ROS_DEBUG("%s", marginalization_flag ? "Non-keyframe" : "Keyframe");
    // 打印调试信息，表示正在解决当前帧
    ROS_DEBUG("Solving %d", frame_count);
    // 打印调试信息，表示当前特征点的数量
    ROS_DEBUG("number of feature: %d", f_manager.getFeatureCount());
    // 记录当前帧的时间戳
    Headers[frame_count] = header;

    // 创建一个新的 ImageFrame 对象，并存储图像数据和时间戳
    ImageFrame imageframe(image, header);
    // 将临时预积分器赋值给当前图像帧
    imageframe.pre_integration = tmp_pre_integration;
    // 将当前图像帧插入到所有图像帧的映射中
    all_image_frame.insert(make_pair(header, imageframe));
    // 创建一个新的临时预积分器对象
    tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

    // 如果正在进行外参标定（ESTIMATE_EXTRINSIC == 2）
    if(ESTIMATE_EXTRINSIC == 2)
    {
        // 打印调试信息，提示正在进行外参标定
        ROS_INFO("calibrating extrinsic param, rotation movement is needed");
        // 如果不是第一帧
        if (frame_count != 0)
        {
            // 获取前后两帧之间的对应特征点
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
            Matrix3d calib_ric;
            // 使用对应的特征点和预积分结果计算外参旋转矩阵
            if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
            {
                // 打印警告信息，提示外参旋转矩阵标定成功
                ROS_WARN("initial extrinsic rotation calib success");
                // 打印外参旋转矩阵
                ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
                // 更新 ric 和 RIC 数组中的第一个元素
                ric[0] = calib_ric;
                RIC[0] = calib_ric;
                // 设置 ESTIMATE_EXTRINSIC 为 1，表示已完成外参标定
                ESTIMATE_EXTRINSIC = 1;
            }
        }
    }

    // 如果当前处于初始化阶段 (solver_flag == INITIAL)
    if (solver_flag == INITIAL)
    {
        // 单目相机 + IMU 初始化
        if (!STEREO && USE_IMU)
        {
            // 当达到窗口大小时进行初始化
            if (frame_count == WINDOW_SIZE)
            {
                bool result = false;
                // 如果未进行外参标定且时间间隔足够大
                if(ESTIMATE_EXTRINSIC != 2 && (header - initial_timestamp) > 0.1)
                {
                    // 进行初始结构估计
                    result = initialStructure();
                    // 更新初始时间戳
                    initial_timestamp = header;   
                }
                // 如果初始化成功
                if(result)
                {
                    // 设置求解器标志为非线性模式
                    solver_flag = NON_LINEAR;
                    // 进行优化
                    optimization();
                    // 滑动窗口
                    slideWindow();
                    // 打印调试信息，提示初始化完成
                    ROS_INFO("Initialization finish!");
                }
                else
                    // 如果初始化失败，滑动窗口并丢弃最旧的数据
                    slideWindow();
            }
        }

		// stereo + IMU initialization
		if(STEREO && USE_IMU)
		{
		    // 使用 PnP 方法初始化当前帧的姿态和位置
		    f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
		    // 三角化当前帧的特征点，恢复其三维位置
		    f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
		    if (frame_count == WINDOW_SIZE)
		    {
		        // 当达到窗口大小时，更新所有图像帧的姿态和位置
		        map<double, ImageFrame>::iterator frame_it;
		        int i = 0;
		        for (frame_it = all_image_frame.begin(); frame_it != all_image_frame.end(); frame_it++)
		        {
		            frame_it->second.R = Rs[i];
		            frame_it->second.T = Ps[i];
		            i++;
		        }
		        // 解算陀螺仪偏置
		        solveGyroscopeBias(all_image_frame, Bgs);
		        // 重新传播预积分器以应用新的陀螺仪偏置
		        for (int i = 0; i <= WINDOW_SIZE; i++)
		        {
		            pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
		        }
		        // 设置求解器标志为非线性优化
		        solver_flag = NON_LINEAR;
		        // 进行非线性优化
		        optimization();
		        // 滑动窗口
		        slideWindow();
		        ROS_INFO("Initialization finish!");
		    }
		}
		
		// stereo only initialization
		if(STEREO && !USE_IMU)
		{
		    // 使用 PnP 方法初始化当前帧的姿态和位置
		    f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
		    // 三角化当前帧的特征点，恢复其三维位置
		    f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
		    // 进行优化
		    optimization();
		
		    if(frame_count == WINDOW_SIZE)
		    {
		        // 设置求解器标志为非线性优化
		        solver_flag = NON_LINEAR;
		        // 滑动窗口
		        slideWindow();
		        ROS_INFO("Initialization finish!");
		    }
		}
		
		// 如果当前帧数小于窗口大小，则复制前一帧的状态
		if(frame_count < WINDOW_SIZE)
		{
		    frame_count++; // 增加帧计数
		    int prev_frame = frame_count - 1;
		    // 复制前一帧的位置、速度、姿态、加速度计偏置和陀螺仪偏置
		    Ps[frame_count] = Ps[prev_frame];
		    Vs[frame_count] = Vs[prev_frame];
		    Rs[frame_count] = Rs[prev_frame];
		    Bas[frame_count] = Bas[prev_frame];
		    Bgs[frame_count] = Bgs[prev_frame];
		}
		else
		{
		    TicToc t_solve; // 计时开始
		
		    // 如果不使用 IMU，则仅使用立体视觉进行姿态初始化
		    if(!USE_IMU)
		        f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
		
		    // 三角化当前帧的特征点，恢复其三维位置
		    f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
		    // 进行优化
		    optimization();
		    // 检测并记录异常特征点索引
		    set<int> removeIndex;
		    outliersRejection(removeIndex);
		    // 移除异常特征点
		    f_manager.removeOutlier(removeIndex);
		
		    // 如果不是多线程模式，则在单线程中处理特征跟踪和预测
		    if (! MULTIPLE_THREAD)
		    {
		        featureTracker.removeOutliers(removeIndex); // 移除跟踪器中的异常特征点
		        predictPtsInNextFrame(); // 预测下一帧的特征点位置
		    }
		
		    // 输出求解时间
		    ROS_DEBUG("solver costs: %fms", t_solve.toc());
		
		    // 检测系统故障
		    if (failureDetection())
		    {
		        ROS_WARN("failure detection!"); // 发出警告
		        failure_occur = 1; // 标记故障发生
		        clearState(); // 清除状态
		        setParameter(); // 重设参数
		        ROS_WARN("system reboot!"); // 发出重启警告
		        return; // 返回并重启系统
		    }
		
		    // 滑动窗口
		    slideWindow();
		    // 移除失败的特征点
		    f_manager.removeFailures();
		
		    // 准备输出 VINS 关键位姿
		    key_poses.clear();
		    for (int i = 0; i <= WINDOW_SIZE; i++)
		        key_poses.push_back(Ps[i]);
		
		    // 更新最新状态
		    last_R = Rs[WINDOW_SIZE];
		    last_P = Ps[WINDOW_SIZE];
		    last_R0 = Rs[0];
		    last_P0 = Ps[0];
		    updateLatestStates();
    }
}
流程和功能总结

  1. 记录新图像的到来：

    • 打印调试信息，表示有新图像到来。
    • 记录添加的特征点数量。

  2. 添加特征点并检查视差：

    • 使用 f_manager.addFeatureCheckParallax 方法添加特征点并检查视差。
    • 根据视差结果判断当前帧是否为关键帧，并设置相应的 marginalization_flag。

  3. 记录调试信息：

    • 打印当前帧是否为关键帧、帧号、特征点数量以及时间戳。

  4. 创建和存储 ImageFrame 对象：

    • 创建 ImageFrame 对象并将其存储在 all_image_frame 中。
    • 创建新的预积分器 tmp_pre_integration 用于下一次处理。

  5. 校准外参：

    • 如果需要校准外参且满足条件，则进行外参校准。
    • 获取对应帧之间的特征点对，并计算初始的旋转矩阵 ric。
    • 更新 ric 和 RIC，并将 ESTIMATE_EXTRINSIC 设置为已完成校准。

  6. 初始化阶段处理：

    • 单目 + IMU 初始化：
      • 如果达到窗口大小，则尝试进行结构初始化。
      • 如果初始化成功，则进行非线性优化和滑动窗口操作，完成初始化。
    • 双目 + IMU 初始化：
      • 使用 PnP 方法初始化姿态和位置。
      • 通过三角化恢复特征点的三维位置。
      • 达到窗口大小后，更新所有图像帧的姿态和位置，解算陀螺仪偏置，并重新传播预积分器。
      • 完成初始化后进行非线性优化和滑动窗口操作。
    • 双目初始化（不使用 IMU）：
      • 使用 PnP 方法初始化姿态和位置。
      • 通过三角化恢复特征点的三维位置。
      • 进行优化。
      • 达到窗口大小后，完成初始化并进行非线性优化和滑动窗口操作。
    • 复制前一帧的状态：
      • 在未达到窗口大小之前，复制前一帧的状态（位置、速度、姿态、加速度计偏置和陀螺仪偏置）。

  7. 正常运行阶段：

    • 如果不使用 IMU，则仅使用立体视觉进行姿态初始化。
    • 通过三角化恢复特征点的三维位置。
    • 进行优化。
    • 检测并移除异常特征点。
    • 如果不是多线程模式，则在单线程中处理特征跟踪和预测。
    • 输出求解时间。
    • 检测系统故障，如有故障则清除状态并重设参数。
    • 滑动窗口并移除失败的特征点。
    • 准备输出 VINS 关键位姿。
    • 更新最新状态。

这个函数的核心功能是通过处理图像帧中的特征点来进行相机姿态和位置的估计，并结合 IMU 数据（如果启用）来提高估计的准确性。它还包括初始化阶段的关键步骤和正常运行阶段的状态维护与优化。

Estimator::processMeasurements

下面是 processMeasurements 函数的详细流程和功能总结，并附带逐行代码注释：

void Estimator::processMeasurements()
{
    while (1)
    {
        //printf("process measurments\n");
        TicToc t_process; // 开始计时处理测量数据的时间

        // 从特征缓冲区中获取最新的特征数据
        pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>>> feature;
        vector<pair<double, Eigen::Vector3d>> accVector, gyrVector; // 存储加速度计和陀螺仪数据

        if(!featureBuf.empty()) // 检查特征缓冲区是否为空
        {
            feature = featureBuf.front(); // 获取特征缓冲区的第一个元素
            curTime = feature.first + td; // 设置当前时间为特征时间加上时间延迟

            // 等待 IMU 数据可用
            while(1)
            {
                if ((!USE_IMU) || IMUAvailable(feature.first + td)) // 如果不使用 IMU 或者 IMU 数据可用
                    break; // 继续处理
                else
                {
                    printf("wait for imu ... \n"); // 打印等待 IMU 数据的消息
                    if (!MULTIPLE_THREAD) // 如果不是多线程模式
                        return; // 返回并退出函数
                    std::chrono::milliseconds dura(5); // 等待 5 毫秒
                    std::this_thread::sleep_for(dura);
                }
            }

            mBuf.lock(); // 锁定互斥锁以访问共享资源
            if(USE_IMU)
                getIMUInterval(prevTime, curTime, accVector, gyrVector); // 获取指定时间间隔内的 IMU 数据
            featureBuf.pop(); // 从特征缓冲区移除已处理的数据
            mBuf.unlock(); // 解锁互斥锁

            if(USE_IMU) // 如果使用 IMU
            {
                if(!initFirstPoseFlag) // 如果尚未初始化第一帧的姿态
                    initFirstIMUPose(accVector); // 初始化第一帧的姿态
                for(size_t i = 0; i < accVector.size(); i++) // 处理每个 IMU 数据点
                {
                    double dt;
                    if(i == 0)
                        dt = accVector[i].first - prevTime; // 计算第一个数据点的时间间隔
                    else if (i == accVector.size() - 1)
                        dt = curTime - accVector[i - 1].first; // 计算最后一个数据点的时间间隔
                    else
                        dt = accVector[i].first - accVector[i - 1].first; // 计算中间数据点的时间间隔
                    processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second); // 处理 IMU 数据
                }
            }

            processImage(feature.second, feature.first); // 处理图像特征数据
            prevTime = curTime; // 更新前一帧的时间为当前帧的时间

            printStatistics(*this, 0); // 打印统计信息

            std_msgs::Header header; // 创建 ROS 消息头
            header.frame_id = "world"; // 设置坐标系为世界坐标系
            header.stamp = ros::Time(feature.first); // 设置时间戳

            pubOdometry(*this, header); // 发布里程计信息
            pubKeyPoses(*this, header); // 发布关键位姿信息
            pubCameraPose(*this, header); // 发布相机姿态信息
            pubPointCloud(*this, header); // 发布点云信息
            pubKeyframe(*this); // 发布关键帧信息
            pubTF(*this, header); // 发布 TF 变换信息

            printf("process measurement time: %f\n", t_process.toc()); // 打印处理测量数据所花费的时间
        }

        if (!MULTIPLE_THREAD) // 如果不是多线程模式
            break; // 退出循环

        std::chrono::milliseconds dura(2); // 等待 2 毫秒
        std::this_thread::sleep_for(dura);
    }
}
流程和功能总结

  1. 无限循环：

    • 进入一个无限循环，持续处理测量数据。

  2. 开始计时：

    • 使用 TicToc 类开始计时处理测量数据的时间。

  3. 获取特征数据：

    • 从特征缓冲区 featureBuf 中获取最新的特征数据。
    • 将特征数据的时间戳加上时间延迟 td 设置为当前时间 curTime。

  4. 等待 IMU 数据：

    • 如果启用了 IMU，检查指定时间 feature.first + td 是否有可用的 IMU 数据。
    • 如果没有可用的 IMU 数据，则打印等待消息并休眠一段时间（5 毫秒），直到数据可用或不在多线程模式下返回。

  5. 锁定互斥锁：

    • 使用 mBuf.lock() 锁定互斥锁以访问共享资源。

  6. 获取 IMU 数据：

    • 如果启用了 IMU，调用 getIMUInterval 方法获取指定时间间隔内的 IMU 数据（加速度计和陀螺仪数据）。
    • 从特征缓冲区 featureBuf 移除已处理的数据。
    • 使用 mBuf.unlock() 解锁互斥锁。

  7. 处理 IMU 数据：

    • 如果启用了 IMU 且尚未初始化第一帧的姿态，则调用 initFirstIMUPose 方法进行初始化。
    • 遍历获取到的 IMU 数据，计算每个数据点之间的时间间隔 dt，并调用 processIMU 方法处理每个 IMU 数据点。

  8. 处理图像特征数据：

    • 调用 processImage 方法处理图像特征数据。

  9. 更新时间：

    • 将 prevTime 更新为当前时间 curTime。

  10. 打印统计信息：

    • 调用 printStatistics 方法打印系统统计信息。

  11. 发布信息：

    • 创建 ROS 消息头 header，设置坐标系为世界坐标系，并设置时间戳。
    • 调用相应的发布方法发布里程计信息、关键位姿信息、相机姿态信息、点云信息、关键帧信息和 TF 变换信息。

  12. 打印处理时间：

    • 打印处理测量数据所花费的时间。

  13. 检查多线程模式：

    • 如果不在多线程模式下，则退出循环。

  14. 休眠：

    • 在多线程模式下，休眠 2 毫秒后继续下一次循环。

这个函数的核心功能是通过不断循环处理图像特征数据和 IMU 数据来维持系统的状态估计，并在必要时进行初始化和优化。它还包括发布各种传感器数据和估计结果的功能。

VINS-FUSION：配置参数说明与配置自己的参数
weixin_38582851的博客
04-30 666
VINS-Fusion 是一个基于优化的多传感器状态估计器，实现了视觉惯性里程计(VIO)和视觉惯性全球导航卫星系统(VI-GNSS)融合。
Vins-Fusion整体框架，数据流分析
u010196944的博客
10-14 3266
Vins-Fusion整体框架，数据流分析
VINS-Fusion
04-02
香港科技大学的项目VINS-Fusion，单目视觉惯性系统的在线时间校准
VINS-FUSION 前端后端代码全详解
热门推荐
lovely_yoshino的博客
07-16 1万+
本次工作 我首先一步步的把代码全部注释了，十分的详细，对于C++和OpenCV的一些操作也进行了详细的注释，对于刚入门的同学应该还是有帮助的。之后我将代码开源，并写了相应的博客进行讲解。 1. 程序入口rosNodeTest.cpp 1.1 定义内容 运行程序时，首先进入的是主程序vins_estimator/src/estimator/rosNodeTest.cpp 里边主要定义了 估计器、 缓存器 、 获取传感器数据的函数 和 一个主函数 // 获得左目的message void img0_callb
VINS-FUSION代码解读【1】——程序入口
Volador的博客
08-30 2370
vins-fusion不像mono那样有三个node，它只有一个node，在rosNodeTest.cpp里。并且这个文件是整个vins-fusion的程序入口。所以我先从这个文件看起。本人基础不太扎实所以会把代码注释的比较详细，也会适当添加对应知识点的解读。以上就是对rosNodeTest.cpp代码的解读。rosNodeTest.cpp的文件级变量。接下来解析sync_process线程。.........
VINS-Fusion源码逐行解析：processMeasurements()及其关于imu数据处理部分
shikaiaixuexi的博客
06-11 925
我们等待IMU数据，通过IMUAvailable函数来判断IMU数据是否在当前时间戳可用，如果使用IMU，那么通过getIMUInterval函数将imu数据读取进开始设置的变量accVector, gyrVector中，然后没有初始化则调用initFirstIMUPose对imu信息进行初始化，最后计算完时间间隔后，调用processIMU函数来完成对IMU数据的处理，至此，有关imu数据的处理流程结束。processIMU函数处理IMU数据，得到预积分值，让我们来看一下源码。
VINS-Fusion源码逐行解析：程序入口rosNodeTest.cpp
shikaiaixuexi的博客
06-09 767
config：存放了程序运行需要的yaml文件，需要什么相机，就用对应的配置文件就行。接下来我们看整个vins-fusion的程序入口，rosNodeTest.cpp。global_fusion：照VINS-Mono新增加的GPS等全局传感器。今天开始阅读VINS-Fusion，以专栏的形式记录一下，防止之后自己忘。docker：顾名思义，使用docker运行VINS-Fusion。loop_fusion：局部回环检测，若需要回环检测需要单独运行。vins_estimator：主要看的地方；
VINS-FUSION-leanrning:VINS-FUSION中文注释版。现有网络上对于VINS-mono的代码已经有很多讲解和注释了，但是对于VINS-FUSION（以下简称VF）的注释还是很少的，刚好本人最近也正在学习VIO的相关知识，所以对VF按照程序执行顺序进行了十分详细的注释，同时为了和大家进行交流学习，所以把相关注释代码进行开源。因水平有限，错误肯定很多，还请各位大佬们指正-源码时代
03-24
VINS-Fusion中文注释版 创新学习用。 十分感谢港科大各位老师的工作。 目前唯一注释了前端VIO部分。 相应博客 参考内容 以及一些其他的博文，在代码中也进行了注释。 基于优化的多传感器状态估计器 VINS-Fusion是...
VINS-Mono+Fusion源码解析系列（十七）：非线性优化
LDST_CSDN的博客
10-25 527
（1）大致思路​ 按照后端流程，VIO初始化initialStructure完成之后，执行非线性优化solveOdometry。（2）代码实现。
超全！SLAM论文与开源代码汇总（激光+视觉+融合）
qq_29462849的博客
10-20 8574
1.代表性视觉SLAM算法论文与开源代码总结2.代表性激光SLAM算法论文与开源代码总结3.代表性激光-视觉融合SLAM算法论文总结激光-视觉-IMU-GPS融合SLAM算法理论与代码讲解...
动态视觉SLAM的亿点点思考（含20项最新开源代码链接）[上篇]
3D视觉工坊
07-30 606
作者：泡椒味的口香糖 | 来源：3D视觉工坊添加微信：dddvisiona，备注：SLAM，拉你入群。文末附行业细分群。0. 笔者个人体会动态环境下的视觉SLAM一直都是研究的重点和难点，但最近动态SLAM的paper越来越少，感觉主要原因是动态SLAM的框架已经固化，很难做出大的创新。现有的模板基本就是使用目标检测或者语义分割网络剔除动态特征点，然后用几何一致性做进一步的验证。笔者最近也在思考突...
VINS-Fusion源码逐行解析：processImage函数最后剩下的几个函数predictPtsInNextFrame()、failureDetection()等
最新发布
shikaiaixuexi的博客
07-16 373
vins-fusion的主要函数到这里也就基本讲完了，跟一遍下来自己熟悉的同时也希望能给大家带来一些方便，之后会继续给大家带来一些slam的相关内容，有什么问题或建议大家可以关注并私信我，博主能解答的一定尽全力解答。接着看下清空状态的函数clearState()，该函数作用是重置整个状态，清空缓冲区、重置各种变量和标志位，确保系统处于初始状态或者清空所有运行状态以备新的运行使用。今天这讲写完后，vins-fusion也基本完成一遍流程，讲完了。
VINS-FUSION-RGBD | |代码程序分析（1）
qq_36738025的博客
01-31 755
由于网上大部分的程序都是针对原来的VINS-fusion的单双目，很少有针对RGBD的解释，正好项目需求，因此在这里对该程序进行分析。读取每一个相机的外参数，当然这里有一个好玩的事情是，在双目的机体系的原点是左目相机，但是在这里却是一个未知的位置。相对与单双木的程序，本程序仅仅针对RGBD的情况，代码也基本上继承了原来的程序。这里面主要的工作就是读取相机的内参数，当然是调用了其他文件夹里面的函数。非常的多的变量与函数，当然在初始化变量的时候运行的是构造函数。基本上就是在原有的参数上加入了深度相机的外参数。
【VSLAM】VINS-Fusion原理及源码解析（一）——初识VINS-Fusion
caiqidong321的博客
08-16 6912
本系列博客以`VINS-Fusion`源码流程为导向，从入口函数一步步讲解前端和后端，最后完成仿真和真机实验部署，希望可以细致入微地对`VINS-Fusion`作一个完整全面的讲解。
vins-fusion代码理解
不知语冰
12-27 658
代码通读了一遍做些总结，肯定有很多理解错了的地方。 从rosNodeTest.cpp开始 main函数 ros::init(argc, argv, "vins_estimator"); // ros节点初始化 ros::NodeHandle n("~"); readParameters(config_file);// 读取参数 estimator.setParameter();// 设置参数 IMU回调函数，将imu测量量分别放到accBuf和gyrBuf，详见imu_callback ros::Subs
VINS学习01——VINS_Fusion官方教程翻译解读
weixin_44168457的博客
11-08 3468
VINS_Fusion 是一个基于优化的多传感器融合定位算法，由港科大开源，在VINS_Fusion基础上改进。融合传感器包括：IMU，双目相机，GPS
vins-fusion解读
bufengzj的博客
01-04 5290
关心vins-fusion，尤其是融合双目与imu那块。 1. 如何安装测试？数据集与自己的相机 1.1 安装测试 1.2 数据集 1.3 自己的相机 目前的相机有小觅相机（带有硬件时间对齐IMU)，realsense，ZED。 2. 论文与资源 代码：https://github.com/HKUST-Aerial-Robotics/VINS-Fusion 文章：T. Q...
Vins-Fusion 学习笔记
qq_43525734的博客
03-18 4362
VINS-Fusion 基本介绍 VINS-Fusion 是继 VINS-Mono 和 VINS-Mobile（单目视觉惯导 SLAM 方案）后，香港科技大学沈劭劼老师开源的双目视觉惯导 SLAM 方案，VINS-Fusion 是一种基于优化的多传感器状态估计器，可实现自主应用（无人机，汽车和 AR / VR）的精确自定位。 VINS-Fusion 是 VINS-Mono 的扩展，支持多种视觉惯性...
VINS-Fusion源码逐行解析：与processImage有关的一部分函数
shikaiaixuexi的博客
07-07 495
然后对于帧id在 frame_count-2 和 frame_count-1 之间有观测数据的特征点，计算视差总和，最后通过帧数，上一次跟踪的特征点数量、长时间跟踪的特征点数量、新特征点数量和计算的视差来判断当前帧是否通过视差检测，如果返回true，则删掉最旧的一帧，如果返回false，则删掉倒数第二帧，这样可以控制优化计算量，只对当前帧之前的一部分帧进行优化，而不是全部历史帧。该函数很简单，我们往下看，到了getCorresponding函数，该函数是在两个指定的帧（
实现matlab控制的贪吃蛇状态估计系统
同时，我们会涉及到在Rviz中使用reusnake_visualize工具来可视化蛇的状态，并利用VINS-fusion算法进行视觉里程计的获取。为了提高状态估计性能，我们还需要对VINS-fusion进行修改，使其能够融合来自蛇传感器的额外...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值