ROS index logo ROS Index
  1. Home
  2. Packages
  3. vins_estimator
 
No version for distro humble showing github. Known supported distros are highlighted in the buttons above.
Package symbol

vins_estimator package from vslam repo

ORB_SLAM2 communicate learning_tf mbot_description mbot_gazebo mbot_teleop rbx1 rbx1_apps rbx1_bringup rbx1_description rbx1_dynamixels rbx1_nav rbx1_speech rbx1_vision robot_vision robot_voice mbot_bringup mbot_navigation marm_description marm_gazebo marm_moveit_config marm_planning follower_velocity_smoother mbot_follower mbot_with_arm_moveit_config example_interfaces ros2_demo plan_and_run actionlib_lutorials agitros cv_camera industrial_core industrial_deprecated industrial_msgs industrial_robot_client industrial_robot_simulator industrial_trajectory_filters industrial_utils simple_message learn_gazebo learn_nav_msg lesson_perception myworkcell_core nodelet_tutorial_math collision_avoidance_pick_and_place manipulation_msgs face_recognition robot_setup_tf ros_arduino_bridge ros_arduino_firmware ros_arduino_msgs ros_arduino_python ros_industrial_learn ros_orb rosserial rosserial_arduino rosserial_client rosserial_embeddedlinux rosserial_mbed rosserial_msgs rosserial_python rosserial_server rosserial_test rosserial_tivac rosserial_windows rosserial_xbee usb_cam using_markers_rviz vision_system voice_system ar_demo benchmark_publisher camera_model feature_tracker pose_graph vins_estimator camera_models global_fusion loop_fusion vins

ROS Distro
github

API Docs
Browse Code

Package Summary

Tags No category tags.
Version 0.0.0
License TODO
Build type CATKIN
Use RECOMMENDED

Repository Summary

Description Vision SLAM study and research
Checkout URI https://github.com/mrwangmaomao/vslam.git
VCS Type git
VCS Version master
Last Updated 2019-04-01
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The vins_estimator package

Additional Links

No additional links.

Maintainers

  • qintong

Authors

No additional authors.
VINS/VINS-Mono-code-annotation-master/vins_estimator/README.md

视觉惯组系统

1 各文件作用

  • factor 实现IMU、camera等残差模型，涉及了ceres优化库
  • initial 系统初始化，外参标定，SFM
  • loop_closure 闭环检测，这里主要利用DBOW2作者的一个demo程序
  • utility 相机显示，四元数转换，系统rviz可视化
  • estimator_node (main()函数) 执行多线程measurement_process、loop_detection、pose_graph
  • feature_manager.cpp特征点管理，三角化，关键帧操作
  • parameters.cpp外部系统设置参数输入

2 estimator_node文件

main函数

step1

初始化”vins_estimator”节点，读取配置文件参数。

step2

发布Rviz相关话题 订阅 “图像特征点数据” “复位estimator” “根据回环检测信息进行重定位”三个话题

step3

执行process()线程函数。主要处理VIO后端，包括IMU预积分、松耦合初始化和局部BA

  • 调用匿名函数，从缓存队列中读取IMU数据和图像特征点数据，如果measurements为空，则匿名函数返回false，调用wait(lock),释放m_buf（为了使图像和IMU回调函数可以访问缓存队列），阻塞当前线程，等待被con.notify_one()唤醒直到measurements不为空时（成功从缓存队列获取数据），匿名函数返回true，则可以退出while循环。
  • 遍历数组中各帧的IMU数据，进行预积分。如果第一帧IMU数据的时间戳晚于图像特征点的时间戳，需要对IMU数据进行插值得到图像帧时间戳对应的IMU数据，然后进行预积分estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
  • 检测是否有回环 重定位，如果有重定位，需要estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
  • 将图像特征点数据存到一个map容器中，键是特征点id，键值是特征点。执行处理图像程序estimator.processImage(image, img_msg->header);
  • 每处理完一帧图像特征点数据，发布话题
  • 检测是否需要优化，VINS系统完成滑动窗口优化后，用优化后的结果，更新里程计数据update();

线程锁的使用

|线程锁|作用| |-|-| |m_buf|处理多个线程使用imu_buf和feature_buf的冲突| |m_state|处理多个线程使用当前里程计信息（即tmp_P、tmp_Q、tmp_V）的冲突| |m_estimator|处理多个线程使用VINS系统对象（即Estimator类的实例estimator）的冲突|

3 预积分 factor/integration_base.h

参考:
https://blog.csdn.net/LilyNothing/article/details/79113841

https://blog.csdn.net/lancelot_vim/article/details/77888788

为什么需要预积分

1

在两帧视觉帧之间，往往有很多IMU的采集数据，其中计算速度积分的时候需要相机当前位姿，将当前位置引入到世界坐标系$C_Ws$，从p时刻数字积分一次得到p+1时刻，然后根据公式乘以p时刻相机位姿得到世界坐标系，最终导致要对k时刻开始的每一个时刻的相机pose求导，计算代价很大，由于状态变了，积分的值也会跟着变，每优化一次就要重新算一次积分。

中点积分

\(w=(w_0+w_1)/2+bg -->更新角速率\) \(R=R_0 \times \frac{w}{2} -->更新旋转\) \(a_0=R_0(a_0^{'}-ba) -->上一次载体的加速度\) \(a_1=R_0(a_1^{'}-ba) -->当前载体的加速度\) \(a=(a_0+a_1)/2 -->更新加速度\) \(\Delta P=\Delta P^{'}+\Delta V^{'}\times dt + \frac{1}{2}a\Delta t^2 -->更新位移\) \(\Delta V=\Delta V^{'} + a\Delta t -->更新速度\)
对应代码(factor/interation_base.h midPointIntegration()函数)

Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;

yujifen

误差状态方程

公式的推导详细看VINS-Mono详解 中的2.2和2.3部分。 雅克比矩阵初始为单位矩阵，协方差矩阵初始为0矩阵。

当加速度计和陀螺仪的偏置发生微小改变时，就可以根据雅克比矩阵对预计积分项进行修正，避免重复积分。

设置F和V矩阵

MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                        -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                        -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Package Dependencies

Deps Name
roscpp
catkin

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

Launch files

  • launch/3dm.launch
      • config_path [default: $(find feature_tracker)/../config/3dm/3dm_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/black_box.launch
      • config_path [default: $(find feature_tracker)/../config/black_box/black_box_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/cla.launch
      • config_path [default: $(find feature_tracker)/../config/cla/cla_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc_no_extrinsic_param.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config_no_extrinsic.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_color.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_color_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_fisheye.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_fisheye_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/tum.launch
      • config_path [default: $(find feature_tracker)/../config/tum/tum_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/vins_rviz.launch

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged vins_estimator at Robotics Stack Exchange

No version for distro jazzy showing github. Known supported distros are highlighted in the buttons above.
Package symbol

vins_estimator package from vslam repo

ORB_SLAM2 communicate learning_tf mbot_description mbot_gazebo mbot_teleop rbx1 rbx1_apps rbx1_bringup rbx1_description rbx1_dynamixels rbx1_nav rbx1_speech rbx1_vision robot_vision robot_voice mbot_bringup mbot_navigation marm_description marm_gazebo marm_moveit_config marm_planning follower_velocity_smoother mbot_follower mbot_with_arm_moveit_config example_interfaces ros2_demo plan_and_run actionlib_lutorials agitros cv_camera industrial_core industrial_deprecated industrial_msgs industrial_robot_client industrial_robot_simulator industrial_trajectory_filters industrial_utils simple_message learn_gazebo learn_nav_msg lesson_perception myworkcell_core nodelet_tutorial_math collision_avoidance_pick_and_place manipulation_msgs face_recognition robot_setup_tf ros_arduino_bridge ros_arduino_firmware ros_arduino_msgs ros_arduino_python ros_industrial_learn ros_orb rosserial rosserial_arduino rosserial_client rosserial_embeddedlinux rosserial_mbed rosserial_msgs rosserial_python rosserial_server rosserial_test rosserial_tivac rosserial_windows rosserial_xbee usb_cam using_markers_rviz vision_system voice_system ar_demo benchmark_publisher camera_model feature_tracker pose_graph vins_estimator camera_models global_fusion loop_fusion vins

ROS Distro
github

API Docs
Browse Code

Package Summary

Tags No category tags.
Version 0.0.0
License TODO
Build type CATKIN
Use RECOMMENDED

Repository Summary

Description Vision SLAM study and research
Checkout URI https://github.com/mrwangmaomao/vslam.git
VCS Type git
VCS Version master
Last Updated 2019-04-01
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The vins_estimator package

Additional Links

No additional links.

Maintainers

  • qintong

Authors

No additional authors.
VINS/VINS-Mono-code-annotation-master/vins_estimator/README.md

视觉惯组系统

1 各文件作用

  • factor 实现IMU、camera等残差模型，涉及了ceres优化库
  • initial 系统初始化，外参标定，SFM
  • loop_closure 闭环检测，这里主要利用DBOW2作者的一个demo程序
  • utility 相机显示，四元数转换，系统rviz可视化
  • estimator_node (main()函数) 执行多线程measurement_process、loop_detection、pose_graph
  • feature_manager.cpp特征点管理，三角化，关键帧操作
  • parameters.cpp外部系统设置参数输入

2 estimator_node文件

main函数

step1

初始化”vins_estimator”节点，读取配置文件参数。

step2

发布Rviz相关话题 订阅 “图像特征点数据” “复位estimator” “根据回环检测信息进行重定位”三个话题

step3

执行process()线程函数。主要处理VIO后端，包括IMU预积分、松耦合初始化和局部BA

  • 调用匿名函数，从缓存队列中读取IMU数据和图像特征点数据，如果measurements为空，则匿名函数返回false，调用wait(lock),释放m_buf（为了使图像和IMU回调函数可以访问缓存队列），阻塞当前线程，等待被con.notify_one()唤醒直到measurements不为空时（成功从缓存队列获取数据），匿名函数返回true，则可以退出while循环。
  • 遍历数组中各帧的IMU数据，进行预积分。如果第一帧IMU数据的时间戳晚于图像特征点的时间戳，需要对IMU数据进行插值得到图像帧时间戳对应的IMU数据，然后进行预积分estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
  • 检测是否有回环 重定位，如果有重定位，需要estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
  • 将图像特征点数据存到一个map容器中，键是特征点id，键值是特征点。执行处理图像程序estimator.processImage(image, img_msg->header);
  • 每处理完一帧图像特征点数据，发布话题
  • 检测是否需要优化，VINS系统完成滑动窗口优化后，用优化后的结果，更新里程计数据update();

线程锁的使用

|线程锁|作用| |-|-| |m_buf|处理多个线程使用imu_buf和feature_buf的冲突| |m_state|处理多个线程使用当前里程计信息（即tmp_P、tmp_Q、tmp_V）的冲突| |m_estimator|处理多个线程使用VINS系统对象（即Estimator类的实例estimator）的冲突|

3 预积分 factor/integration_base.h

参考:
https://blog.csdn.net/LilyNothing/article/details/79113841

https://blog.csdn.net/lancelot_vim/article/details/77888788

为什么需要预积分

1

在两帧视觉帧之间，往往有很多IMU的采集数据，其中计算速度积分的时候需要相机当前位姿，将当前位置引入到世界坐标系$C_Ws$，从p时刻数字积分一次得到p+1时刻，然后根据公式乘以p时刻相机位姿得到世界坐标系，最终导致要对k时刻开始的每一个时刻的相机pose求导，计算代价很大，由于状态变了，积分的值也会跟着变，每优化一次就要重新算一次积分。

中点积分

\(w=(w_0+w_1)/2+bg -->更新角速率\) \(R=R_0 \times \frac{w}{2} -->更新旋转\) \(a_0=R_0(a_0^{'}-ba) -->上一次载体的加速度\) \(a_1=R_0(a_1^{'}-ba) -->当前载体的加速度\) \(a=(a_0+a_1)/2 -->更新加速度\) \(\Delta P=\Delta P^{'}+\Delta V^{'}\times dt + \frac{1}{2}a\Delta t^2 -->更新位移\) \(\Delta V=\Delta V^{'} + a\Delta t -->更新速度\)
对应代码(factor/interation_base.h midPointIntegration()函数)

Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;

yujifen

误差状态方程

公式的推导详细看VINS-Mono详解 中的2.2和2.3部分。 雅克比矩阵初始为单位矩阵，协方差矩阵初始为0矩阵。

当加速度计和陀螺仪的偏置发生微小改变时，就可以根据雅克比矩阵对预计积分项进行修正，避免重复积分。

设置F和V矩阵

MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                        -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                        -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Package Dependencies

Deps Name
roscpp
catkin

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

Launch files

  • launch/3dm.launch
      • config_path [default: $(find feature_tracker)/../config/3dm/3dm_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/black_box.launch
      • config_path [default: $(find feature_tracker)/../config/black_box/black_box_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/cla.launch
      • config_path [default: $(find feature_tracker)/../config/cla/cla_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc_no_extrinsic_param.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config_no_extrinsic.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_color.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_color_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_fisheye.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_fisheye_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/tum.launch
      • config_path [default: $(find feature_tracker)/../config/tum/tum_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/vins_rviz.launch

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged vins_estimator at Robotics Stack Exchange

No version for distro kilted showing github. Known supported distros are highlighted in the buttons above.
Package symbol

vins_estimator package from vslam repo

ORB_SLAM2 communicate learning_tf mbot_description mbot_gazebo mbot_teleop rbx1 rbx1_apps rbx1_bringup rbx1_description rbx1_dynamixels rbx1_nav rbx1_speech rbx1_vision robot_vision robot_voice mbot_bringup mbot_navigation marm_description marm_gazebo marm_moveit_config marm_planning follower_velocity_smoother mbot_follower mbot_with_arm_moveit_config example_interfaces ros2_demo plan_and_run actionlib_lutorials agitros cv_camera industrial_core industrial_deprecated industrial_msgs industrial_robot_client industrial_robot_simulator industrial_trajectory_filters industrial_utils simple_message learn_gazebo learn_nav_msg lesson_perception myworkcell_core nodelet_tutorial_math collision_avoidance_pick_and_place manipulation_msgs face_recognition robot_setup_tf ros_arduino_bridge ros_arduino_firmware ros_arduino_msgs ros_arduino_python ros_industrial_learn ros_orb rosserial rosserial_arduino rosserial_client rosserial_embeddedlinux rosserial_mbed rosserial_msgs rosserial_python rosserial_server rosserial_test rosserial_tivac rosserial_windows rosserial_xbee usb_cam using_markers_rviz vision_system voice_system ar_demo benchmark_publisher camera_model feature_tracker pose_graph vins_estimator camera_models global_fusion loop_fusion vins

ROS Distro
github

API Docs
Browse Code

Package Summary

Tags No category tags.
Version 0.0.0
License TODO
Build type CATKIN
Use RECOMMENDED

Repository Summary

Description Vision SLAM study and research
Checkout URI https://github.com/mrwangmaomao/vslam.git
VCS Type git
VCS Version master
Last Updated 2019-04-01
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The vins_estimator package

Additional Links

No additional links.

Maintainers

  • qintong

Authors

No additional authors.
VINS/VINS-Mono-code-annotation-master/vins_estimator/README.md

视觉惯组系统

1 各文件作用

  • factor 实现IMU、camera等残差模型，涉及了ceres优化库
  • initial 系统初始化，外参标定，SFM
  • loop_closure 闭环检测，这里主要利用DBOW2作者的一个demo程序
  • utility 相机显示，四元数转换，系统rviz可视化
  • estimator_node (main()函数) 执行多线程measurement_process、loop_detection、pose_graph
  • feature_manager.cpp特征点管理，三角化，关键帧操作
  • parameters.cpp外部系统设置参数输入

2 estimator_node文件

main函数

step1

初始化”vins_estimator”节点，读取配置文件参数。

step2

发布Rviz相关话题 订阅 “图像特征点数据” “复位estimator” “根据回环检测信息进行重定位”三个话题

step3

执行process()线程函数。主要处理VIO后端，包括IMU预积分、松耦合初始化和局部BA

  • 调用匿名函数，从缓存队列中读取IMU数据和图像特征点数据，如果measurements为空，则匿名函数返回false，调用wait(lock),释放m_buf（为了使图像和IMU回调函数可以访问缓存队列），阻塞当前线程，等待被con.notify_one()唤醒直到measurements不为空时（成功从缓存队列获取数据），匿名函数返回true，则可以退出while循环。
  • 遍历数组中各帧的IMU数据，进行预积分。如果第一帧IMU数据的时间戳晚于图像特征点的时间戳，需要对IMU数据进行插值得到图像帧时间戳对应的IMU数据，然后进行预积分estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
  • 检测是否有回环 重定位，如果有重定位，需要estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
  • 将图像特征点数据存到一个map容器中，键是特征点id，键值是特征点。执行处理图像程序estimator.processImage(image, img_msg->header);
  • 每处理完一帧图像特征点数据，发布话题
  • 检测是否需要优化，VINS系统完成滑动窗口优化后，用优化后的结果，更新里程计数据update();

线程锁的使用

|线程锁|作用| |-|-| |m_buf|处理多个线程使用imu_buf和feature_buf的冲突| |m_state|处理多个线程使用当前里程计信息（即tmp_P、tmp_Q、tmp_V）的冲突| |m_estimator|处理多个线程使用VINS系统对象（即Estimator类的实例estimator）的冲突|

3 预积分 factor/integration_base.h

参考:
https://blog.csdn.net/LilyNothing/article/details/79113841

https://blog.csdn.net/lancelot_vim/article/details/77888788

为什么需要预积分

1

在两帧视觉帧之间，往往有很多IMU的采集数据，其中计算速度积分的时候需要相机当前位姿，将当前位置引入到世界坐标系$C_Ws$，从p时刻数字积分一次得到p+1时刻，然后根据公式乘以p时刻相机位姿得到世界坐标系，最终导致要对k时刻开始的每一个时刻的相机pose求导，计算代价很大，由于状态变了，积分的值也会跟着变，每优化一次就要重新算一次积分。

中点积分

\(w=(w_0+w_1)/2+bg -->更新角速率\) \(R=R_0 \times \frac{w}{2} -->更新旋转\) \(a_0=R_0(a_0^{'}-ba) -->上一次载体的加速度\) \(a_1=R_0(a_1^{'}-ba) -->当前载体的加速度\) \(a=(a_0+a_1)/2 -->更新加速度\) \(\Delta P=\Delta P^{'}+\Delta V^{'}\times dt + \frac{1}{2}a\Delta t^2 -->更新位移\) \(\Delta V=\Delta V^{'} + a\Delta t -->更新速度\)
对应代码(factor/interation_base.h midPointIntegration()函数)

Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;

yujifen

误差状态方程

公式的推导详细看VINS-Mono详解 中的2.2和2.3部分。 雅克比矩阵初始为单位矩阵，协方差矩阵初始为0矩阵。

当加速度计和陀螺仪的偏置发生微小改变时，就可以根据雅克比矩阵对预计积分项进行修正，避免重复积分。

设置F和V矩阵

MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                        -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                        -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Package Dependencies

Deps Name
roscpp
catkin

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

Launch files

  • launch/3dm.launch
      • config_path [default: $(find feature_tracker)/../config/3dm/3dm_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/black_box.launch
      • config_path [default: $(find feature_tracker)/../config/black_box/black_box_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/cla.launch
      • config_path [default: $(find feature_tracker)/../config/cla/cla_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc_no_extrinsic_param.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config_no_extrinsic.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_color.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_color_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_fisheye.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_fisheye_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/tum.launch
      • config_path [default: $(find feature_tracker)/../config/tum/tum_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/vins_rviz.launch

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged vins_estimator at Robotics Stack Exchange

No version for distro rolling showing github. Known supported distros are highlighted in the buttons above.
Package symbol

vins_estimator package from vslam repo

ORB_SLAM2 communicate learning_tf mbot_description mbot_gazebo mbot_teleop rbx1 rbx1_apps rbx1_bringup rbx1_description rbx1_dynamixels rbx1_nav rbx1_speech rbx1_vision robot_vision robot_voice mbot_bringup mbot_navigation marm_description marm_gazebo marm_moveit_config marm_planning follower_velocity_smoother mbot_follower mbot_with_arm_moveit_config example_interfaces ros2_demo plan_and_run actionlib_lutorials agitros cv_camera industrial_core industrial_deprecated industrial_msgs industrial_robot_client industrial_robot_simulator industrial_trajectory_filters industrial_utils simple_message learn_gazebo learn_nav_msg lesson_perception myworkcell_core nodelet_tutorial_math collision_avoidance_pick_and_place manipulation_msgs face_recognition robot_setup_tf ros_arduino_bridge ros_arduino_firmware ros_arduino_msgs ros_arduino_python ros_industrial_learn ros_orb rosserial rosserial_arduino rosserial_client rosserial_embeddedlinux rosserial_mbed rosserial_msgs rosserial_python rosserial_server rosserial_test rosserial_tivac rosserial_windows rosserial_xbee usb_cam using_markers_rviz vision_system voice_system ar_demo benchmark_publisher camera_model feature_tracker pose_graph vins_estimator camera_models global_fusion loop_fusion vins

ROS Distro
github

API Docs
Browse Code

Package Summary

Tags No category tags.
Version 0.0.0
License TODO
Build type CATKIN
Use RECOMMENDED

Repository Summary

Description Vision SLAM study and research
Checkout URI https://github.com/mrwangmaomao/vslam.git
VCS Type git
VCS Version master
Last Updated 2019-04-01
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The vins_estimator package

Additional Links

No additional links.

Maintainers

  • qintong

Authors

No additional authors.
VINS/VINS-Mono-code-annotation-master/vins_estimator/README.md

视觉惯组系统

1 各文件作用

  • factor 实现IMU、camera等残差模型，涉及了ceres优化库
  • initial 系统初始化，外参标定，SFM
  • loop_closure 闭环检测，这里主要利用DBOW2作者的一个demo程序
  • utility 相机显示，四元数转换，系统rviz可视化
  • estimator_node (main()函数) 执行多线程measurement_process、loop_detection、pose_graph
  • feature_manager.cpp特征点管理，三角化，关键帧操作
  • parameters.cpp外部系统设置参数输入

2 estimator_node文件

main函数

step1

初始化”vins_estimator”节点，读取配置文件参数。

step2

发布Rviz相关话题 订阅 “图像特征点数据” “复位estimator” “根据回环检测信息进行重定位”三个话题

step3

执行process()线程函数。主要处理VIO后端，包括IMU预积分、松耦合初始化和局部BA

  • 调用匿名函数，从缓存队列中读取IMU数据和图像特征点数据，如果measurements为空，则匿名函数返回false，调用wait(lock),释放m_buf（为了使图像和IMU回调函数可以访问缓存队列），阻塞当前线程，等待被con.notify_one()唤醒直到measurements不为空时（成功从缓存队列获取数据），匿名函数返回true，则可以退出while循环。
  • 遍历数组中各帧的IMU数据，进行预积分。如果第一帧IMU数据的时间戳晚于图像特征点的时间戳，需要对IMU数据进行插值得到图像帧时间戳对应的IMU数据，然后进行预积分estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
  • 检测是否有回环 重定位，如果有重定位，需要estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
  • 将图像特征点数据存到一个map容器中，键是特征点id，键值是特征点。执行处理图像程序estimator.processImage(image, img_msg->header);
  • 每处理完一帧图像特征点数据，发布话题
  • 检测是否需要优化，VINS系统完成滑动窗口优化后，用优化后的结果，更新里程计数据update();

线程锁的使用

|线程锁|作用| |-|-| |m_buf|处理多个线程使用imu_buf和feature_buf的冲突| |m_state|处理多个线程使用当前里程计信息（即tmp_P、tmp_Q、tmp_V）的冲突| |m_estimator|处理多个线程使用VINS系统对象（即Estimator类的实例estimator）的冲突|

3 预积分 factor/integration_base.h

参考:
https://blog.csdn.net/LilyNothing/article/details/79113841

https://blog.csdn.net/lancelot_vim/article/details/77888788

为什么需要预积分

1

在两帧视觉帧之间，往往有很多IMU的采集数据，其中计算速度积分的时候需要相机当前位姿，将当前位置引入到世界坐标系$C_Ws$，从p时刻数字积分一次得到p+1时刻，然后根据公式乘以p时刻相机位姿得到世界坐标系，最终导致要对k时刻开始的每一个时刻的相机pose求导，计算代价很大，由于状态变了，积分的值也会跟着变，每优化一次就要重新算一次积分。

中点积分

\(w=(w_0+w_1)/2+bg -->更新角速率\) \(R=R_0 \times \frac{w}{2} -->更新旋转\) \(a_0=R_0(a_0^{'}-ba) -->上一次载体的加速度\) \(a_1=R_0(a_1^{'}-ba) -->当前载体的加速度\) \(a=(a_0+a_1)/2 -->更新加速度\) \(\Delta P=\Delta P^{'}+\Delta V^{'}\times dt + \frac{1}{2}a\Delta t^2 -->更新位移\) \(\Delta V=\Delta V^{'} + a\Delta t -->更新速度\)
对应代码(factor/interation_base.h midPointIntegration()函数)

Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;

yujifen

误差状态方程

公式的推导详细看VINS-Mono详解 中的2.2和2.3部分。 雅克比矩阵初始为单位矩阵，协方差矩阵初始为0矩阵。

当加速度计和陀螺仪的偏置发生微小改变时，就可以根据雅克比矩阵对预计积分项进行修正，避免重复积分。

设置F和V矩阵

MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                        -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                        -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Package Dependencies

Deps Name
roscpp
catkin

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

Launch files

  • launch/3dm.launch
      • config_path [default: $(find feature_tracker)/../config/3dm/3dm_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/black_box.launch
      • config_path [default: $(find feature_tracker)/../config/black_box/black_box_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/cla.launch
      • config_path [default: $(find feature_tracker)/../config/cla/cla_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc_no_extrinsic_param.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config_no_extrinsic.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_color.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_color_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_fisheye.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_fisheye_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/tum.launch
      • config_path [default: $(find feature_tracker)/../config/tum/tum_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/vins_rviz.launch

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged vins_estimator at Robotics Stack Exchange

Package symbol

vins_estimator package from vslam repo

ORB_SLAM2 communicate learning_tf mbot_description mbot_gazebo mbot_teleop rbx1 rbx1_apps rbx1_bringup rbx1_description rbx1_dynamixels rbx1_nav rbx1_speech rbx1_vision robot_vision robot_voice mbot_bringup mbot_navigation marm_description marm_gazebo marm_moveit_config marm_planning follower_velocity_smoother mbot_follower mbot_with_arm_moveit_config example_interfaces ros2_demo plan_and_run actionlib_lutorials agitros cv_camera industrial_core industrial_deprecated industrial_msgs industrial_robot_client industrial_robot_simulator industrial_trajectory_filters industrial_utils simple_message learn_gazebo learn_nav_msg lesson_perception myworkcell_core nodelet_tutorial_math collision_avoidance_pick_and_place manipulation_msgs face_recognition robot_setup_tf ros_arduino_bridge ros_arduino_firmware ros_arduino_msgs ros_arduino_python ros_industrial_learn ros_orb rosserial rosserial_arduino rosserial_client rosserial_embeddedlinux rosserial_mbed rosserial_msgs rosserial_python rosserial_server rosserial_test rosserial_tivac rosserial_windows rosserial_xbee usb_cam using_markers_rviz vision_system voice_system ar_demo benchmark_publisher camera_model feature_tracker pose_graph vins_estimator camera_models global_fusion loop_fusion vins

ROS Distro
github

API Docs
Browse Code

Package Summary

Tags No category tags.
Version 0.0.0
License TODO
Build type CATKIN
Use RECOMMENDED

Repository Summary

Description Vision SLAM study and research
Checkout URI https://github.com/mrwangmaomao/vslam.git
VCS Type git
VCS Version master
Last Updated 2019-04-01
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The vins_estimator package

Additional Links

No additional links.

Maintainers

  • qintong

Authors

No additional authors.
VINS/VINS-Mono-code-annotation-master/vins_estimator/README.md

视觉惯组系统

1 各文件作用

  • factor 实现IMU、camera等残差模型，涉及了ceres优化库
  • initial 系统初始化，外参标定，SFM
  • loop_closure 闭环检测，这里主要利用DBOW2作者的一个demo程序
  • utility 相机显示，四元数转换，系统rviz可视化
  • estimator_node (main()函数) 执行多线程measurement_process、loop_detection、pose_graph
  • feature_manager.cpp特征点管理，三角化，关键帧操作
  • parameters.cpp外部系统设置参数输入

2 estimator_node文件

main函数

step1

初始化”vins_estimator”节点，读取配置文件参数。

step2

发布Rviz相关话题 订阅 “图像特征点数据” “复位estimator” “根据回环检测信息进行重定位”三个话题

step3

执行process()线程函数。主要处理VIO后端，包括IMU预积分、松耦合初始化和局部BA

  • 调用匿名函数，从缓存队列中读取IMU数据和图像特征点数据，如果measurements为空，则匿名函数返回false，调用wait(lock),释放m_buf（为了使图像和IMU回调函数可以访问缓存队列），阻塞当前线程，等待被con.notify_one()唤醒直到measurements不为空时（成功从缓存队列获取数据），匿名函数返回true，则可以退出while循环。
  • 遍历数组中各帧的IMU数据，进行预积分。如果第一帧IMU数据的时间戳晚于图像特征点的时间戳，需要对IMU数据进行插值得到图像帧时间戳对应的IMU数据，然后进行预积分estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
  • 检测是否有回环 重定位，如果有重定位，需要estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
  • 将图像特征点数据存到一个map容器中，键是特征点id，键值是特征点。执行处理图像程序estimator.processImage(image, img_msg->header);
  • 每处理完一帧图像特征点数据，发布话题
  • 检测是否需要优化，VINS系统完成滑动窗口优化后，用优化后的结果，更新里程计数据update();

线程锁的使用

|线程锁|作用| |-|-| |m_buf|处理多个线程使用imu_buf和feature_buf的冲突| |m_state|处理多个线程使用当前里程计信息（即tmp_P、tmp_Q、tmp_V）的冲突| |m_estimator|处理多个线程使用VINS系统对象（即Estimator类的实例estimator）的冲突|

3 预积分 factor/integration_base.h

参考:
https://blog.csdn.net/LilyNothing/article/details/79113841

https://blog.csdn.net/lancelot_vim/article/details/77888788

为什么需要预积分

1

在两帧视觉帧之间，往往有很多IMU的采集数据，其中计算速度积分的时候需要相机当前位姿，将当前位置引入到世界坐标系$C_Ws$，从p时刻数字积分一次得到p+1时刻，然后根据公式乘以p时刻相机位姿得到世界坐标系，最终导致要对k时刻开始的每一个时刻的相机pose求导，计算代价很大，由于状态变了，积分的值也会跟着变，每优化一次就要重新算一次积分。

中点积分

\(w=(w_0+w_1)/2+bg -->更新角速率\) \(R=R_0 \times \frac{w}{2} -->更新旋转\) \(a_0=R_0(a_0^{'}-ba) -->上一次载体的加速度\) \(a_1=R_0(a_1^{'}-ba) -->当前载体的加速度\) \(a=(a_0+a_1)/2 -->更新加速度\) \(\Delta P=\Delta P^{'}+\Delta V^{'}\times dt + \frac{1}{2}a\Delta t^2 -->更新位移\) \(\Delta V=\Delta V^{'} + a\Delta t -->更新速度\)
对应代码(factor/interation_base.h midPointIntegration()函数)

Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;

yujifen

误差状态方程

公式的推导详细看VINS-Mono详解 中的2.2和2.3部分。 雅克比矩阵初始为单位矩阵，协方差矩阵初始为0矩阵。

当加速度计和陀螺仪的偏置发生微小改变时，就可以根据雅克比矩阵对预计积分项进行修正，避免重复积分。

设置F和V矩阵

MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                        -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                        -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Package Dependencies

Deps Name
roscpp
catkin

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

Launch files

  • launch/3dm.launch
      • config_path [default: $(find feature_tracker)/../config/3dm/3dm_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/black_box.launch
      • config_path [default: $(find feature_tracker)/../config/black_box/black_box_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/cla.launch
      • config_path [default: $(find feature_tracker)/../config/cla/cla_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc_no_extrinsic_param.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config_no_extrinsic.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_color.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_color_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_fisheye.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_fisheye_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/tum.launch
      • config_path [default: $(find feature_tracker)/../config/tum/tum_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/vins_rviz.launch

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged vins_estimator at Robotics Stack Exchange

No version for distro galactic showing github. Known supported distros are highlighted in the buttons above.
Package symbol

vins_estimator package from vslam repo

ORB_SLAM2 communicate learning_tf mbot_description mbot_gazebo mbot_teleop rbx1 rbx1_apps rbx1_bringup rbx1_description rbx1_dynamixels rbx1_nav rbx1_speech rbx1_vision robot_vision robot_voice mbot_bringup mbot_navigation marm_description marm_gazebo marm_moveit_config marm_planning follower_velocity_smoother mbot_follower mbot_with_arm_moveit_config example_interfaces ros2_demo plan_and_run actionlib_lutorials agitros cv_camera industrial_core industrial_deprecated industrial_msgs industrial_robot_client industrial_robot_simulator industrial_trajectory_filters industrial_utils simple_message learn_gazebo learn_nav_msg lesson_perception myworkcell_core nodelet_tutorial_math collision_avoidance_pick_and_place manipulation_msgs face_recognition robot_setup_tf ros_arduino_bridge ros_arduino_firmware ros_arduino_msgs ros_arduino_python ros_industrial_learn ros_orb rosserial rosserial_arduino rosserial_client rosserial_embeddedlinux rosserial_mbed rosserial_msgs rosserial_python rosserial_server rosserial_test rosserial_tivac rosserial_windows rosserial_xbee usb_cam using_markers_rviz vision_system voice_system ar_demo benchmark_publisher camera_model feature_tracker pose_graph vins_estimator camera_models global_fusion loop_fusion vins

ROS Distro
github

API Docs
Browse Code

Package Summary

Tags No category tags.
Version 0.0.0
License TODO
Build type CATKIN
Use RECOMMENDED

Repository Summary

Description Vision SLAM study and research
Checkout URI https://github.com/mrwangmaomao/vslam.git
VCS Type git
VCS Version master
Last Updated 2019-04-01
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The vins_estimator package

Additional Links

No additional links.

Maintainers

  • qintong

Authors

No additional authors.
VINS/VINS-Mono-code-annotation-master/vins_estimator/README.md

视觉惯组系统

1 各文件作用

  • factor 实现IMU、camera等残差模型，涉及了ceres优化库
  • initial 系统初始化，外参标定，SFM
  • loop_closure 闭环检测，这里主要利用DBOW2作者的一个demo程序
  • utility 相机显示，四元数转换，系统rviz可视化
  • estimator_node (main()函数) 执行多线程measurement_process、loop_detection、pose_graph
  • feature_manager.cpp特征点管理，三角化，关键帧操作
  • parameters.cpp外部系统设置参数输入

2 estimator_node文件

main函数

step1

初始化”vins_estimator”节点，读取配置文件参数。

step2

发布Rviz相关话题 订阅 “图像特征点数据” “复位estimator” “根据回环检测信息进行重定位”三个话题

step3

执行process()线程函数。主要处理VIO后端，包括IMU预积分、松耦合初始化和局部BA

  • 调用匿名函数，从缓存队列中读取IMU数据和图像特征点数据，如果measurements为空，则匿名函数返回false，调用wait(lock),释放m_buf（为了使图像和IMU回调函数可以访问缓存队列），阻塞当前线程，等待被con.notify_one()唤醒直到measurements不为空时（成功从缓存队列获取数据），匿名函数返回true，则可以退出while循环。
  • 遍历数组中各帧的IMU数据，进行预积分。如果第一帧IMU数据的时间戳晚于图像特征点的时间戳，需要对IMU数据进行插值得到图像帧时间戳对应的IMU数据，然后进行预积分estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
  • 检测是否有回环 重定位，如果有重定位，需要estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
  • 将图像特征点数据存到一个map容器中，键是特征点id，键值是特征点。执行处理图像程序estimator.processImage(image, img_msg->header);
  • 每处理完一帧图像特征点数据，发布话题
  • 检测是否需要优化，VINS系统完成滑动窗口优化后，用优化后的结果，更新里程计数据update();

线程锁的使用

|线程锁|作用| |-|-| |m_buf|处理多个线程使用imu_buf和feature_buf的冲突| |m_state|处理多个线程使用当前里程计信息（即tmp_P、tmp_Q、tmp_V）的冲突| |m_estimator|处理多个线程使用VINS系统对象（即Estimator类的实例estimator）的冲突|

3 预积分 factor/integration_base.h

参考:
https://blog.csdn.net/LilyNothing/article/details/79113841

https://blog.csdn.net/lancelot_vim/article/details/77888788

为什么需要预积分

1

在两帧视觉帧之间，往往有很多IMU的采集数据，其中计算速度积分的时候需要相机当前位姿，将当前位置引入到世界坐标系$C_Ws$，从p时刻数字积分一次得到p+1时刻，然后根据公式乘以p时刻相机位姿得到世界坐标系，最终导致要对k时刻开始的每一个时刻的相机pose求导，计算代价很大，由于状态变了，积分的值也会跟着变，每优化一次就要重新算一次积分。

中点积分

\(w=(w_0+w_1)/2+bg -->更新角速率\) \(R=R_0 \times \frac{w}{2} -->更新旋转\) \(a_0=R_0(a_0^{'}-ba) -->上一次载体的加速度\) \(a_1=R_0(a_1^{'}-ba) -->当前载体的加速度\) \(a=(a_0+a_1)/2 -->更新加速度\) \(\Delta P=\Delta P^{'}+\Delta V^{'}\times dt + \frac{1}{2}a\Delta t^2 -->更新位移\) \(\Delta V=\Delta V^{'} + a\Delta t -->更新速度\)
对应代码(factor/interation_base.h midPointIntegration()函数)

Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;

yujifen

误差状态方程

公式的推导详细看VINS-Mono详解 中的2.2和2.3部分。 雅克比矩阵初始为单位矩阵，协方差矩阵初始为0矩阵。

当加速度计和陀螺仪的偏置发生微小改变时，就可以根据雅克比矩阵对预计积分项进行修正，避免重复积分。

设置F和V矩阵

MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                        -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                        -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Package Dependencies

Deps Name
roscpp
catkin

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

Launch files

  • launch/3dm.launch
      • config_path [default: $(find feature_tracker)/../config/3dm/3dm_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/black_box.launch
      • config_path [default: $(find feature_tracker)/../config/black_box/black_box_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/cla.launch
      • config_path [default: $(find feature_tracker)/../config/cla/cla_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc_no_extrinsic_param.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config_no_extrinsic.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_color.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_color_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_fisheye.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_fisheye_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/tum.launch
      • config_path [default: $(find feature_tracker)/../config/tum/tum_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/vins_rviz.launch

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged vins_estimator at Robotics Stack Exchange

No version for distro iron showing github. Known supported distros are highlighted in the buttons above.
Package symbol

vins_estimator package from vslam repo

ORB_SLAM2 communicate learning_tf mbot_description mbot_gazebo mbot_teleop rbx1 rbx1_apps rbx1_bringup rbx1_description rbx1_dynamixels rbx1_nav rbx1_speech rbx1_vision robot_vision robot_voice mbot_bringup mbot_navigation marm_description marm_gazebo marm_moveit_config marm_planning follower_velocity_smoother mbot_follower mbot_with_arm_moveit_config example_interfaces ros2_demo plan_and_run actionlib_lutorials agitros cv_camera industrial_core industrial_deprecated industrial_msgs industrial_robot_client industrial_robot_simulator industrial_trajectory_filters industrial_utils simple_message learn_gazebo learn_nav_msg lesson_perception myworkcell_core nodelet_tutorial_math collision_avoidance_pick_and_place manipulation_msgs face_recognition robot_setup_tf ros_arduino_bridge ros_arduino_firmware ros_arduino_msgs ros_arduino_python ros_industrial_learn ros_orb rosserial rosserial_arduino rosserial_client rosserial_embeddedlinux rosserial_mbed rosserial_msgs rosserial_python rosserial_server rosserial_test rosserial_tivac rosserial_windows rosserial_xbee usb_cam using_markers_rviz vision_system voice_system ar_demo benchmark_publisher camera_model feature_tracker pose_graph vins_estimator camera_models global_fusion loop_fusion vins

ROS Distro
github

API Docs
Browse Code

Package Summary

Tags No category tags.
Version 0.0.0
License TODO
Build type CATKIN
Use RECOMMENDED

Repository Summary

Description Vision SLAM study and research
Checkout URI https://github.com/mrwangmaomao/vslam.git
VCS Type git
VCS Version master
Last Updated 2019-04-01
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The vins_estimator package

Additional Links

No additional links.

Maintainers

  • qintong

Authors

No additional authors.
VINS/VINS-Mono-code-annotation-master/vins_estimator/README.md

视觉惯组系统

1 各文件作用

  • factor 实现IMU、camera等残差模型，涉及了ceres优化库
  • initial 系统初始化，外参标定，SFM
  • loop_closure 闭环检测，这里主要利用DBOW2作者的一个demo程序
  • utility 相机显示，四元数转换，系统rviz可视化
  • estimator_node (main()函数) 执行多线程measurement_process、loop_detection、pose_graph
  • feature_manager.cpp特征点管理，三角化，关键帧操作
  • parameters.cpp外部系统设置参数输入

2 estimator_node文件

main函数

step1

初始化”vins_estimator”节点，读取配置文件参数。

step2

发布Rviz相关话题 订阅 “图像特征点数据” “复位estimator” “根据回环检测信息进行重定位”三个话题

step3

执行process()线程函数。主要处理VIO后端，包括IMU预积分、松耦合初始化和局部BA

  • 调用匿名函数，从缓存队列中读取IMU数据和图像特征点数据，如果measurements为空，则匿名函数返回false，调用wait(lock),释放m_buf（为了使图像和IMU回调函数可以访问缓存队列），阻塞当前线程，等待被con.notify_one()唤醒直到measurements不为空时（成功从缓存队列获取数据），匿名函数返回true，则可以退出while循环。
  • 遍历数组中各帧的IMU数据，进行预积分。如果第一帧IMU数据的时间戳晚于图像特征点的时间戳，需要对IMU数据进行插值得到图像帧时间戳对应的IMU数据，然后进行预积分estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
  • 检测是否有回环 重定位，如果有重定位，需要estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
  • 将图像特征点数据存到一个map容器中，键是特征点id，键值是特征点。执行处理图像程序estimator.processImage(image, img_msg->header);
  • 每处理完一帧图像特征点数据，发布话题
  • 检测是否需要优化，VINS系统完成滑动窗口优化后，用优化后的结果，更新里程计数据update();

线程锁的使用

|线程锁|作用| |-|-| |m_buf|处理多个线程使用imu_buf和feature_buf的冲突| |m_state|处理多个线程使用当前里程计信息（即tmp_P、tmp_Q、tmp_V）的冲突| |m_estimator|处理多个线程使用VINS系统对象（即Estimator类的实例estimator）的冲突|

3 预积分 factor/integration_base.h

参考:
https://blog.csdn.net/LilyNothing/article/details/79113841

https://blog.csdn.net/lancelot_vim/article/details/77888788

为什么需要预积分

1

在两帧视觉帧之间，往往有很多IMU的采集数据，其中计算速度积分的时候需要相机当前位姿，将当前位置引入到世界坐标系$C_Ws$，从p时刻数字积分一次得到p+1时刻，然后根据公式乘以p时刻相机位姿得到世界坐标系，最终导致要对k时刻开始的每一个时刻的相机pose求导，计算代价很大，由于状态变了，积分的值也会跟着变，每优化一次就要重新算一次积分。

中点积分

\(w=(w_0+w_1)/2+bg -->更新角速率\) \(R=R_0 \times \frac{w}{2} -->更新旋转\) \(a_0=R_0(a_0^{'}-ba) -->上一次载体的加速度\) \(a_1=R_0(a_1^{'}-ba) -->当前载体的加速度\) \(a=(a_0+a_1)/2 -->更新加速度\) \(\Delta P=\Delta P^{'}+\Delta V^{'}\times dt + \frac{1}{2}a\Delta t^2 -->更新位移\) \(\Delta V=\Delta V^{'} + a\Delta t -->更新速度\)
对应代码(factor/interation_base.h midPointIntegration()函数)

Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;

yujifen

误差状态方程

公式的推导详细看VINS-Mono详解 中的2.2和2.3部分。 雅克比矩阵初始为单位矩阵，协方差矩阵初始为0矩阵。

当加速度计和陀螺仪的偏置发生微小改变时，就可以根据雅克比矩阵对预计积分项进行修正，避免重复积分。

设置F和V矩阵

MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                        -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                        -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Package Dependencies

Deps Name
roscpp
catkin

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

Launch files

  • launch/3dm.launch
      • config_path [default: $(find feature_tracker)/../config/3dm/3dm_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/black_box.launch
      • config_path [default: $(find feature_tracker)/../config/black_box/black_box_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/cla.launch
      • config_path [default: $(find feature_tracker)/../config/cla/cla_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc_no_extrinsic_param.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config_no_extrinsic.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_color.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_color_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_fisheye.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_fisheye_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/tum.launch
      • config_path [default: $(find feature_tracker)/../config/tum/tum_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/vins_rviz.launch

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged vins_estimator at Robotics Stack Exchange

No version for distro melodic showing github. Known supported distros are highlighted in the buttons above.
Package symbol

vins_estimator package from vslam repo

ORB_SLAM2 communicate learning_tf mbot_description mbot_gazebo mbot_teleop rbx1 rbx1_apps rbx1_bringup rbx1_description rbx1_dynamixels rbx1_nav rbx1_speech rbx1_vision robot_vision robot_voice mbot_bringup mbot_navigation marm_description marm_gazebo marm_moveit_config marm_planning follower_velocity_smoother mbot_follower mbot_with_arm_moveit_config example_interfaces ros2_demo plan_and_run actionlib_lutorials agitros cv_camera industrial_core industrial_deprecated industrial_msgs industrial_robot_client industrial_robot_simulator industrial_trajectory_filters industrial_utils simple_message learn_gazebo learn_nav_msg lesson_perception myworkcell_core nodelet_tutorial_math collision_avoidance_pick_and_place manipulation_msgs face_recognition robot_setup_tf ros_arduino_bridge ros_arduino_firmware ros_arduino_msgs ros_arduino_python ros_industrial_learn ros_orb rosserial rosserial_arduino rosserial_client rosserial_embeddedlinux rosserial_mbed rosserial_msgs rosserial_python rosserial_server rosserial_test rosserial_tivac rosserial_windows rosserial_xbee usb_cam using_markers_rviz vision_system voice_system ar_demo benchmark_publisher camera_model feature_tracker pose_graph vins_estimator camera_models global_fusion loop_fusion vins

ROS Distro
github

API Docs
Browse Code
Wiki

Package Summary

Tags No category tags.
Version 0.0.0
License TODO
Build type CATKIN
Use RECOMMENDED

Repository Summary

Description Vision SLAM study and research
Checkout URI https://github.com/mrwangmaomao/vslam.git
VCS Type git
VCS Version master
Last Updated 2019-04-01
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The vins_estimator package

Additional Links

No additional links.

Maintainers

  • qintong

Authors

No additional authors.
VINS/VINS-Mono-code-annotation-master/vins_estimator/README.md

视觉惯组系统

1 各文件作用

  • factor 实现IMU、camera等残差模型，涉及了ceres优化库
  • initial 系统初始化，外参标定，SFM
  • loop_closure 闭环检测，这里主要利用DBOW2作者的一个demo程序
  • utility 相机显示，四元数转换，系统rviz可视化
  • estimator_node (main()函数) 执行多线程measurement_process、loop_detection、pose_graph
  • feature_manager.cpp特征点管理，三角化，关键帧操作
  • parameters.cpp外部系统设置参数输入

2 estimator_node文件

main函数

step1

初始化”vins_estimator”节点，读取配置文件参数。

step2

发布Rviz相关话题 订阅 “图像特征点数据” “复位estimator” “根据回环检测信息进行重定位”三个话题

step3

执行process()线程函数。主要处理VIO后端，包括IMU预积分、松耦合初始化和局部BA

  • 调用匿名函数，从缓存队列中读取IMU数据和图像特征点数据，如果measurements为空，则匿名函数返回false，调用wait(lock),释放m_buf（为了使图像和IMU回调函数可以访问缓存队列），阻塞当前线程，等待被con.notify_one()唤醒直到measurements不为空时（成功从缓存队列获取数据），匿名函数返回true，则可以退出while循环。
  • 遍历数组中各帧的IMU数据，进行预积分。如果第一帧IMU数据的时间戳晚于图像特征点的时间戳，需要对IMU数据进行插值得到图像帧时间戳对应的IMU数据，然后进行预积分estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
  • 检测是否有回环 重定位，如果有重定位，需要estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
  • 将图像特征点数据存到一个map容器中，键是特征点id，键值是特征点。执行处理图像程序estimator.processImage(image, img_msg->header);
  • 每处理完一帧图像特征点数据，发布话题
  • 检测是否需要优化，VINS系统完成滑动窗口优化后，用优化后的结果，更新里程计数据update();

线程锁的使用

|线程锁|作用| |-|-| |m_buf|处理多个线程使用imu_buf和feature_buf的冲突| |m_state|处理多个线程使用当前里程计信息（即tmp_P、tmp_Q、tmp_V）的冲突| |m_estimator|处理多个线程使用VINS系统对象（即Estimator类的实例estimator）的冲突|

3 预积分 factor/integration_base.h

参考:
https://blog.csdn.net/LilyNothing/article/details/79113841

https://blog.csdn.net/lancelot_vim/article/details/77888788

为什么需要预积分

1

在两帧视觉帧之间，往往有很多IMU的采集数据，其中计算速度积分的时候需要相机当前位姿，将当前位置引入到世界坐标系$C_Ws$，从p时刻数字积分一次得到p+1时刻，然后根据公式乘以p时刻相机位姿得到世界坐标系，最终导致要对k时刻开始的每一个时刻的相机pose求导，计算代价很大，由于状态变了，积分的值也会跟着变，每优化一次就要重新算一次积分。

中点积分

\(w=(w_0+w_1)/2+bg -->更新角速率\) \(R=R_0 \times \frac{w}{2} -->更新旋转\) \(a_0=R_0(a_0^{'}-ba) -->上一次载体的加速度\) \(a_1=R_0(a_1^{'}-ba) -->当前载体的加速度\) \(a=(a_0+a_1)/2 -->更新加速度\) \(\Delta P=\Delta P^{'}+\Delta V^{'}\times dt + \frac{1}{2}a\Delta t^2 -->更新位移\) \(\Delta V=\Delta V^{'} + a\Delta t -->更新速度\)
对应代码(factor/interation_base.h midPointIntegration()函数)

Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;

yujifen

误差状态方程

公式的推导详细看VINS-Mono详解 中的2.2和2.3部分。 雅克比矩阵初始为单位矩阵，协方差矩阵初始为0矩阵。

当加速度计和陀螺仪的偏置发生微小改变时，就可以根据雅克比矩阵对预计积分项进行修正，避免重复积分。

设置F和V矩阵

MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                        -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                        -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Package Dependencies

Deps Name
roscpp
catkin

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

Launch files

  • launch/3dm.launch
      • config_path [default: $(find feature_tracker)/../config/3dm/3dm_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/black_box.launch
      • config_path [default: $(find feature_tracker)/../config/black_box/black_box_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/cla.launch
      • config_path [default: $(find feature_tracker)/../config/cla/cla_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc_no_extrinsic_param.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config_no_extrinsic.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_color.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_color_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_fisheye.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_fisheye_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/tum.launch
      • config_path [default: $(find feature_tracker)/../config/tum/tum_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/vins_rviz.launch

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged vins_estimator at Robotics Stack Exchange

No version for distro noetic showing github. Known supported distros are highlighted in the buttons above.
Package symbol

vins_estimator package from vslam repo

ORB_SLAM2 communicate learning_tf mbot_description mbot_gazebo mbot_teleop rbx1 rbx1_apps rbx1_bringup rbx1_description rbx1_dynamixels rbx1_nav rbx1_speech rbx1_vision robot_vision robot_voice mbot_bringup mbot_navigation marm_description marm_gazebo marm_moveit_config marm_planning follower_velocity_smoother mbot_follower mbot_with_arm_moveit_config example_interfaces ros2_demo plan_and_run actionlib_lutorials agitros cv_camera industrial_core industrial_deprecated industrial_msgs industrial_robot_client industrial_robot_simulator industrial_trajectory_filters industrial_utils simple_message learn_gazebo learn_nav_msg lesson_perception myworkcell_core nodelet_tutorial_math collision_avoidance_pick_and_place manipulation_msgs face_recognition robot_setup_tf ros_arduino_bridge ros_arduino_firmware ros_arduino_msgs ros_arduino_python ros_industrial_learn ros_orb rosserial rosserial_arduino rosserial_client rosserial_embeddedlinux rosserial_mbed rosserial_msgs rosserial_python rosserial_server rosserial_test rosserial_tivac rosserial_windows rosserial_xbee usb_cam using_markers_rviz vision_system voice_system ar_demo benchmark_publisher camera_model feature_tracker pose_graph vins_estimator camera_models global_fusion loop_fusion vins

ROS Distro
github

API Docs
Browse Code
Wiki

Package Summary

Tags No category tags.
Version 0.0.0
License TODO
Build type CATKIN
Use RECOMMENDED

Repository Summary

Description Vision SLAM study and research
Checkout URI https://github.com/mrwangmaomao/vslam.git
VCS Type git
VCS Version master
Last Updated 2019-04-01
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The vins_estimator package

Additional Links

No additional links.

Maintainers

  • qintong

Authors

No additional authors.
VINS/VINS-Mono-code-annotation-master/vins_estimator/README.md

视觉惯组系统

1 各文件作用

  • factor 实现IMU、camera等残差模型，涉及了ceres优化库
  • initial 系统初始化，外参标定，SFM
  • loop_closure 闭环检测，这里主要利用DBOW2作者的一个demo程序
  • utility 相机显示，四元数转换，系统rviz可视化
  • estimator_node (main()函数) 执行多线程measurement_process、loop_detection、pose_graph
  • feature_manager.cpp特征点管理，三角化，关键帧操作
  • parameters.cpp外部系统设置参数输入

2 estimator_node文件

main函数

step1

初始化”vins_estimator”节点，读取配置文件参数。

step2

发布Rviz相关话题 订阅 “图像特征点数据” “复位estimator” “根据回环检测信息进行重定位”三个话题

step3

执行process()线程函数。主要处理VIO后端，包括IMU预积分、松耦合初始化和局部BA

  • 调用匿名函数，从缓存队列中读取IMU数据和图像特征点数据，如果measurements为空，则匿名函数返回false，调用wait(lock),释放m_buf（为了使图像和IMU回调函数可以访问缓存队列），阻塞当前线程，等待被con.notify_one()唤醒直到measurements不为空时（成功从缓存队列获取数据），匿名函数返回true，则可以退出while循环。
  • 遍历数组中各帧的IMU数据，进行预积分。如果第一帧IMU数据的时间戳晚于图像特征点的时间戳，需要对IMU数据进行插值得到图像帧时间戳对应的IMU数据，然后进行预积分estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
  • 检测是否有回环 重定位，如果有重定位，需要estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
  • 将图像特征点数据存到一个map容器中，键是特征点id，键值是特征点。执行处理图像程序estimator.processImage(image, img_msg->header);
  • 每处理完一帧图像特征点数据，发布话题
  • 检测是否需要优化，VINS系统完成滑动窗口优化后，用优化后的结果，更新里程计数据update();

线程锁的使用

|线程锁|作用| |-|-| |m_buf|处理多个线程使用imu_buf和feature_buf的冲突| |m_state|处理多个线程使用当前里程计信息（即tmp_P、tmp_Q、tmp_V）的冲突| |m_estimator|处理多个线程使用VINS系统对象（即Estimator类的实例estimator）的冲突|

3 预积分 factor/integration_base.h

参考:
https://blog.csdn.net/LilyNothing/article/details/79113841

https://blog.csdn.net/lancelot_vim/article/details/77888788

为什么需要预积分

1

在两帧视觉帧之间，往往有很多IMU的采集数据，其中计算速度积分的时候需要相机当前位姿，将当前位置引入到世界坐标系$C_Ws$，从p时刻数字积分一次得到p+1时刻，然后根据公式乘以p时刻相机位姿得到世界坐标系，最终导致要对k时刻开始的每一个时刻的相机pose求导，计算代价很大，由于状态变了，积分的值也会跟着变，每优化一次就要重新算一次积分。

中点积分

\(w=(w_0+w_1)/2+bg -->更新角速率\) \(R=R_0 \times \frac{w}{2} -->更新旋转\) \(a_0=R_0(a_0^{'}-ba) -->上一次载体的加速度\) \(a_1=R_0(a_1^{'}-ba) -->当前载体的加速度\) \(a=(a_0+a_1)/2 -->更新加速度\) \(\Delta P=\Delta P^{'}+\Delta V^{'}\times dt + \frac{1}{2}a\Delta t^2 -->更新位移\) \(\Delta V=\Delta V^{'} + a\Delta t -->更新速度\)
对应代码(factor/interation_base.h midPointIntegration()函数)

Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;

yujifen

误差状态方程

公式的推导详细看VINS-Mono详解 中的2.2和2.3部分。 雅克比矩阵初始为单位矩阵，协方差矩阵初始为0矩阵。

当加速度计和陀螺仪的偏置发生微小改变时，就可以根据雅克比矩阵对预计积分项进行修正，避免重复积分。

设置F和V矩阵

MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                        -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                        -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Package Dependencies

Deps Name
roscpp
catkin

System Dependencies

No direct system dependencies.

Dependant Packages

No known dependants.

Launch files

  • launch/3dm.launch
      • config_path [default: $(find feature_tracker)/../config/3dm/3dm_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/black_box.launch
      • config_path [default: $(find feature_tracker)/../config/black_box/black_box_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/cla.launch
      • config_path [default: $(find feature_tracker)/../config/cla/cla_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/euroc_no_extrinsic_param.launch
      • config_path [default: $(find feature_tracker)/../config/euroc/euroc_config_no_extrinsic.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_color.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_color_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/realsense_fisheye.launch
      • config_path [default: $(find feature_tracker)/../config/realsense/realsense_fisheye_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/tum.launch
      • config_path [default: $(find feature_tracker)/../config/tum/tum_config.yaml]
      • vins_path [default: $(find feature_tracker)/../config/../]
  • launch/vins_rviz.launch

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged vins_estimator at Robotics Stack Exchange