R3LIVE源码解析(8) — R3LIVE中r3live.cpp文件

目录

1 r3live.cpp简介

2 r3live.cpp源码解析

3 r3live.hpp源码解析


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("/cloud_registered", 100);
        pubLaserCloudEffect = m_ros_node_handle.advertise("/cloud_effected", 100);
        pubLaserCloudMap = m_ros_node_handle.advertise("/Laser_map", 100);
        pubOdomAftMapped = m_ros_node_handle.advertise("/aft_mapped_to_init", 10);
        pub_track_img = m_ros_node_handle.advertise("/track_img",1000);
        pub_raw_img = m_ros_node_handle.advertise("/raw_in_img",1000);
        m_pub_visual_tracked_3d_pts = m_ros_node_handle.advertise("/track_pts", 10);
        m_pub_render_rgb_pts = m_ros_node_handle.advertise("/render_pts", 10);
        pubPath = m_ros_node_handle.advertise("/path", 10);

        pub_odom_cam = m_ros_node_handle.advertise("/camera_odom", 10);
        pub_path_cam = m_ros_node_handle.advertise("/camera_path", 10);
        std::string LiDAR_pointcloud_topic, IMU_topic, IMAGE_topic, IMAGE_topic_compressed;

        //(2)从ros参数服务器上获取参数
        get_ros_parameter(m_ros_node_handle, "/LiDAR_pointcloud_topic", LiDAR_pointcloud_topic, std::string("/laser_cloud_flat") );
        get_ros_parameter(m_ros_node_handle, "/IMU_topic", IMU_topic, std::string("/livox/imu") );
        get_ros_parameter(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(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();
        cube_points_add = boost::make_shared();
        laserCloudFullRes2 = boost::make_shared();
        laserCloudFullResColor = boost::make_shared>();

        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 
    void pointBodyToWorld(const Eigen::Matrix &pi, Eigen::Matrix &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_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子系统处理线程。

你可能感兴趣的:(R3LIVE项目实战,R3LIVE,源码解析)