目录
1 r3live.cpp简介
我们在R3LIVE流程解析中提到R3LIVE主要由两个node节点所依赖的cpp文件组成，我们在上一节中完成了r3live_LiDAR_front_end 简单介绍，下面我们需要详细的看/r3live_mapping这个节点。这个节点也是我们R3LIVE的核心。 r3live.cpp文件的主要是进行一系列的初始化，然后创建LIO线程。
2 r3live.cpp源码解析
r3live.cpp文件比较简洁，输出一些打印信息和初始化，主函数最重要的一点就是new R3LIVE(); // 创建一个R3LIVE对象。
Camera_Lidar_queue g_camera_lidar_queue;
MeasureGroup Measures; // 存放雷达数据和IMU数据的结构体变量
StatesGroup g_lio_state; // lidar状态变量(dim=29)
std::string data_dump_dir = std::string("/mnt/0B3B134F0B3B134F/color_temp_r3live/");
int main(int argc, char **argv)
{
printf_program("R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package");
Common_tools::printf_software_version();
Eigen::initParallel(); // 初始化Eigen库，用于进行并行计算。
ros::init(argc, argv, "R3LIVE_main"); //初始化ROS
R3LIVE * fast_lio_instance = new R3LIVE(); // 创建一个R3LIVE对象
ros::Rate rate(5000);
bool status = ros::ok();
ros::spin();
}我们到r3live.hpp文件中来详细看一下。
3 r3live.hpp源码解析
r3live.hpp文件存在一系列的参数初始化，创建一系列Topic，用于订阅和发布信息，获取一些param信息，打印一些基本的信息，最后是一系列函数的声明。
R3LIVE()
{
//(1)初始化涉及到的发布消息格式
pubLaserCloudFullRes = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/cloud_registered", 100);
pubLaserCloudEffect = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/cloud_effected", 100);
pubLaserCloudMap = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/Laser_map", 100);
pubOdomAftMapped = m_ros_node_handle.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 10);
pub_track_img = m_ros_node_handle.advertise<sensor_msgs::Image>("/track_img",1000);
pub_raw_img = m_ros_node_handle.advertise<sensor_msgs::Image>("/raw_in_img",1000);
m_pub_visual_tracked_3d_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/track_pts", 10);
m_pub_render_rgb_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/render_pts", 10);
pubPath = m_ros_node_handle.advertise<nav_msgs::Path>("/path", 10);
pub_odom_cam = m_ros_node_handle.advertise<nav_msgs::Odometry>("/camera_odom", 10);
pub_path_cam = m_ros_node_handle.advertise<nav_msgs::Path>("/camera_path", 10);
std::string LiDAR_pointcloud_topic, IMU_topic, IMAGE_topic, IMAGE_topic_compressed;
//(2)从ros参数服务器上获取参数
get_ros_parameter<std::string>(m_ros_node_handle, "/LiDAR_pointcloud_topic", LiDAR_pointcloud_topic, std::string("/laser_cloud_flat") );
get_ros_parameter<std::string>(m_ros_node_handle, "/IMU_topic", IMU_topic, std::string("/livox/imu") );
get_ros_parameter<std::string>(m_ros_node_handle, "/Image_topic", IMAGE_topic, std::string("/camera/image_color") );
IMAGE_topic_compressed = std::string(IMAGE_topic).append("/compressed");
//(3)打印基本信息
if(1)
{
scope_color(ANSI_COLOR_BLUE_BOLD);
cout << "======= Summary of subscribed topics =======" << endl;
cout << "LiDAR pointcloud topic: " << LiDAR_pointcloud_topic << endl;
cout << "IMU topic: " << IMU_topic << endl;
cout << "Image topic: " << IMAGE_topic << endl;
cout << "Image compressed topic: " << IMAGE_topic << endl;
cout << "======= -End- =======" << endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
//(4)设置IMU,Lidar,image的订阅信息和回调函数
//(4-1) IMU回调函数 : 用于向LIO和VIO子系统的IMU缓冲中插入IMU数据
sub_imu = m_ros_node_handle.subscribe(IMU_topic.c_str(), 2000000, &R3LIVE::imu_cbk, this, ros::TransportHints().tcpNoDelay());
//(4-2) Lidar回调函数 : 用于向Lidar_buffer中加入接收到的雷达数据 : 注意这里的LiDAR_pointcloud_topic是经过了原始数据转换后的lidar面特征数据.
sub_pcl = m_ros_node_handle.subscribe(LiDAR_pointcloud_topic.c_str(), 2000000, &R3LIVE::feat_points_cbk, this, ros::TransportHints().tcpNoDelay());
//(4-3) Image回调函数 :
sub_img = m_ros_node_handle.subscribe(IMAGE_topic.c_str(), 1000000, &R3LIVE::image_callback, this, ros::TransportHints().tcpNoDelay());
sub_img_comp = m_ros_node_handle.subscribe(IMAGE_topic_compressed.c_str(), 1000000, &R3LIVE::image_comp_callback, this, ros::TransportHints().tcpNoDelay());
m_ros_node_handle.getParam("/initial_pose", m_initial_pose);
m_pub_rgb_render_pointcloud_ptr_vec.resize(1e3);
// (5) 从ROS参数服务器上获取参数信息
// ANCHOR - ROS parameters
if ( 1 )
{
scope_color( ANSI_COLOR_RED );
get_ros_parameter( m_ros_node_handle, "r3live_common/map_output_dir", m_map_output_dir,
Common_tools::get_home_folder().append( "/r3live_output" ) );
get_ros_parameter( m_ros_node_handle, "r3live_common/append_global_map_point_step", m_append_global_map_point_step, 4 );
get_ros_parameter( m_ros_node_handle, "r3live_common/recent_visited_voxel_activated_time", m_recent_visited_voxel_activated_time, 0.0 );
get_ros_parameter( m_ros_node_handle, "r3live_common/maximum_image_buffer", m_maximum_image_buffer, 20000 );
get_ros_parameter( m_ros_node_handle, "r3live_common/tracker_minimum_depth", m_tracker_minimum_depth, 0.1 );
get_ros_parameter( m_ros_node_handle, "r3live_common/tracker_maximum_depth", m_tracker_maximum_depth, 200.0 );
get_ros_parameter( m_ros_node_handle, "r3live_common/track_windows_size", m_track_windows_size, 40 );
get_ros_parameter( m_ros_node_handle, "r3live_common/minimum_pts_size", m_minumum_rgb_pts_size, 0.05 );
get_ros_parameter( m_ros_node_handle, "r3live_common/record_offline_map", m_if_record_mvs, 0 );
get_ros_parameter( m_ros_node_handle, "r3live_common/pub_pt_minimum_views", m_pub_pt_minimum_views, 5 );
get_ros_parameter( m_ros_node_handle, "r3live_common/image_downsample_ratio", m_image_downsample_ratio, 1.0 );
get_ros_parameter( m_ros_node_handle, "r3live_common/esikf_iter_times", esikf_iter_times, 2 );
get_ros_parameter( m_ros_node_handle, "r3live_common/estimate_i2c_extrinsic", m_if_estimate_i2c_extrinsic, 0 );
get_ros_parameter( m_ros_node_handle, "r3live_common/estimate_intrinsic", m_if_estimate_intrinsic, 0 );
get_ros_parameter( m_ros_node_handle, "r3live_common/maximum_vio_tracked_pts", m_maximum_vio_tracked_pts, 600 );
}
if ( 1 )
{
scope_color( ANSI_COLOR_GREEN );
get_ros_parameter( m_ros_node_handle, "r3live_lio/dense_map_enable", dense_map_en, true );
get_ros_parameter( m_ros_node_handle, "r3live_lio/lidar_time_delay", m_lidar_imu_time_delay, 0.0 );
get_ros_parameter( m_ros_node_handle, "r3live_lio/max_iteration", NUM_MAX_ITERATIONS, 4 );
get_ros_parameter( m_ros_node_handle, "r3live_lio/fov_degree", fov_deg, 360.00 );
get_ros_parameter( m_ros_node_handle, "r3live_lio/voxel_downsample_size_surf", m_voxel_downsample_size_surf, 0.3 );
get_ros_parameter( m_ros_node_handle, "r3live_lio/voxel_downsample_size_axis_z", m_voxel_downsample_size_axis_z,
m_voxel_downsample_size_surf );
get_ros_parameter( m_ros_node_handle, "r3live_lio/filter_size_map", filter_size_map_min, 0.4 );
get_ros_parameter( m_ros_node_handle, "r3live_lio/cube_side_length", cube_len, 10000000.0 );
get_ros_parameter( m_ros_node_handle, "r3live_lio/maximum_pt_kdtree_dis", m_maximum_pt_kdtree_dis, 0.5 );
get_ros_parameter( m_ros_node_handle, "r3live_lio/maximum_res_dis", m_maximum_res_dis, 0.3 );
get_ros_parameter( m_ros_node_handle, "r3live_lio/planar_check_dis", m_planar_check_dis, 0.10 );
get_ros_parameter( m_ros_node_handle, "r3live_lio/long_rang_pt_dis", m_long_rang_pt_dis, 500.0 );
get_ros_parameter( m_ros_node_handle, "r3live_lio/publish_feature_map", m_if_publish_feature_map, false );
get_ros_parameter( m_ros_node_handle, "r3live_lio/lio_update_point_step", m_lio_update_point_step, 1 );
}
if ( 1 )
{
scope_color( ANSI_COLOR_BLUE );
load_vio_parameters();
}
// (6)创建地图输出路径
if(!Common_tools::if_file_exist(m_map_output_dir))
{
cout << ANSI_COLOR_BLUE_BOLD << "Create r3live output dir: " << m_map_output_dir << ANSI_COLOR_RESET << endl;
Common_tools::create_dir(m_map_output_dir);
}
// (7)初始化用到的变量
m_thread_pool_ptr = std::make_shared<Common_tools::ThreadPool>(6, true, false); // 设置线程池 At least 5 threads are needs, here we allocate 6 threads.
g_cost_time_logger.init_log( std::string(m_map_output_dir).append("/cost_time_logger.log"));
m_map_rgb_pts.set_minmum_dis(m_minumum_rgb_pts_size);
m_map_rgb_pts.m_recent_visited_voxel_activated_time = m_recent_visited_voxel_activated_time;
featsFromMap = boost::make_shared<PointCloudXYZINormal>();
cube_points_add = boost::make_shared<PointCloudXYZINormal>();
laserCloudFullRes2 = boost::make_shared<PointCloudXYZINormal>();
laserCloudFullResColor = boost::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
XAxisPoint_body = Eigen::Vector3f(LIDAR_SP_LEN, 0.0, 0.0); // (2, 0, 0)
XAxisPoint_world = Eigen::Vector3f(LIDAR_SP_LEN, 0.0, 0.0);// (2, 0, 0)
downSizeFilterSurf.setLeafSize(m_voxel_downsample_size_surf, m_voxel_downsample_size_surf, m_voxel_downsample_size_axis_z);
downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min);
// (8)设置LIO的log文件, 创建LIO子系统线程
m_lio_state_fp = fopen( std::string(m_map_output_dir).append("/lic_lio.log").c_str(), "w+");
m_lio_costtime_fp = fopen(std::string(m_map_output_dir).append("/lic_lio_costtime.log").c_str(), "w+");
m_thread_pool_ptr->commit_task(&R3LIVE::service_LIO_update, this); // 创建LIO线程
}
~R3LIVE(){};
//project lidar frame to world
void pointBodyToWorld(PointType const *const pi, PointType *const po);
template <typename T>
void pointBodyToWorld(const Eigen::Matrix<T, 3, 1> &pi, Eigen::Matrix<T, 3, 1> &po)
{
Eigen::Vector3d p_body(pi[0], pi[1], pi[2]);
Eigen::Vector3d p_global(g_lio_state.rot_end * (p_body + Lidar_offset_to_IMU) + g_lio_state.pos_end);
po[0] = p_global(0);
po[1] = p_global(1);
po[2] = p_global(2);
}
void RGBpointBodyToWorld(PointType const *const pi, pcl::PointXYZI *const po);
int get_cube_index(const int &i, const int &j, const int &k);
bool center_in_FOV(Eigen::Vector3f cube_p);
bool if_corner_in_FOV(Eigen::Vector3f cube_p);
void lasermap_fov_segment();
void feat_points_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg_in);
void wait_render_thread_finish();
bool get_pointcloud_data_from_ros_message(sensor_msgs::PointCloud2::ConstPtr & msg, pcl::PointCloud<pcl::PointXYZINormal> & pcl_pc);
int service_LIO_update();
void publish_render_pts( ros::Publisher &pts_pub, Global_map &m_map_rgb_pts );
void print_dash_board();
};
值得注意的是，在r3live.hpp同文件中，作者就完成了LIO线程的创建。
// (8)设置LIO的log文件, 创建LIO子系统线程
m_lio_state_fp = fopen( std::string(m_map_output_dir).append("/lic_lio.log").c_str(), "w+");
m_lio_costtime_fp = fopen(std::string(m_map_output_dir).append("/lic_lio_costtime.log").c_str(), "w+");
m_thread_pool_ptr->commit_task(&R3LIVE::service_LIO_update, this); // 创建LIO线程m_thread_pool_ptr->commit_task() 调用了m_thread_pool_ptr指向的线程池对象的commit_task方法。
- m_thread_pool_ptr：这是一个指针变量，指向一个线程池对象。线程池是一种用来管理和调度多个线程执行任务的机制。
- commit_task方法：这是线程池对象的一个成员方法，用于提交任务给线程池进行执行。它接受一个函数指针和一个参数作为参数。
- R3LIVE::service_LIO_update：这是一个函数指针，是LIO子系统处理线程。