多传感器融合定位（GPS+Wheel+camera）（2）**FilterFusionSystem类**介绍

多传感器融合定位（GPS+Wheel+camera）（2）FilterFusionSystem类介绍

1、系统初始化读取参数

FilterFusionSystem::FilterFusionSystem(const std::string& param_file) 
    : initialized_(false), odom_G_R_O_(Eigen::Matrix3d::Identity()), odom_G_p_O_(Eigen::Vector3d::Zero()) {
    /// Load parameters.  从配置文件中读取参数
    LoadParam(param_file, &param_);

    config_.compute_raw_odom_ = param_.sys_config.compute_raw_odom;
    config_.sliding_window_size_ = param_.sys_config.sliding_window_size;
    config_.enable_plane_update = param_.sys_config.enable_plane_update;
    config_.enable_gps_update = param_.sys_config.enable_gps_update;

    /// Initialize all modules.
    //初始化所有模块
    //TGK::DataSynchronizer是命名空间，WheelImageSynchronizer是类
    data_sync_ = std::make_unique<TGK::DataSynchronizer::WheelImageSynchronizer>();
    //初始化类包括图像到里程计的外参和车轮的内参
    initializer_ = std::make_unique<Initializer>(param_.extrinsic.O_R_C, param_.extrinsic.O_p_C, 
        param_.wheel_param.kl, param_.wheel_param.kr, param_.wheel_param.b);
   //里程计传播
    propagator_ = std::make_unique<Propagator>(param_.wheel_param.kl, param_.wheel_param.kr, param_.wheel_param.b, 
                                               param_.wheel_param.noise_factor);
       //针孔相机模型
       camera_ = std::make_shared<TGK::Camera::PinholeRadTanCamera>(
        param_.cam_intrinsic.width, param_.cam_intrinsic.height,
        param_.cam_intrinsic.fx, param_.cam_intrinsic.fy,
        param_.cam_intrinsic.cx, param_.cam_intrinsic.cy,
        param_.cam_intrinsic.k1, param_.cam_intrinsic.k2,
        param_.cam_intrinsic.p1, param_.cam_intrinsic.p2,
        param_.cam_intrinsic.k3);
    //三角化
    const auto triangulator = std::make_shared<TGK::Geometry::Triangulator>(param_.tri_config, camera_);

    // Create feature tracker.    创建特征跟踪器
    Eigen::Matrix<double, 8, 1> cam_intrin;//相机内参
    cam_intrin << param_.cam_intrinsic.fx, param_.cam_intrinsic.fy,
                  param_.cam_intrinsic.cx, param_.cam_intrinsic.cy,
                  param_.cam_intrinsic.k1, param_.cam_intrinsic.k2,
                  param_.cam_intrinsic.p1, param_.cam_intrinsic.p2;
   //KLT特征跟踪
    feature_tracker_ = std::make_shared<TGK::ImageProcessor::KLTFeatureTracker>(param_.tracker_config);
   //仿真特征跟踪
    sim_feature_tracker_ = std::make_shared<TGK::ImageProcessor::SimFeatureTrakcer>();  
   //视觉更新
    visual_updater_ = std::make_unique<VisualUpdater>(param_.visual_updater_config, camera_, feature_tracker_, triangulator);
    //使能平面更新
    if (config_.enable_plane_update) {
        plane_updater_ = std::make_unique<PlaneUpdater>(param_.plane_updater_config);
    } 
   //使能GPS更新
    if (config_.enable_gps_update) {
        gps_updater_ = std::make_unique<GpsUpdater>(param_.extrinsic.C_p_Gps);//GPS->Camera的外参
    }

    viz_ = std::make_unique<Visualizer>(param_.viz_config);//可视化配置
   //计算原始里程计
    if (config_.compute_raw_odom_) {
        wheel_propagator_ = std::make_unique<TGK::WheelProcessor::WheelPropagator>(
            param_.wheel_param.kl, param_.wheel_param.kr, param_.wheel_param.b);
    }
}

加载配置参数

void LoadParam(const std::string& param_file, Parameter* params) {
    cv::FileStorage cv_params(param_file, cv::FileStorage::READ);
    
    // Load camera param. 读取相机内参
    params->cam_intrinsic.fx = cv_params["Camera.fx"];
    params->cam_intrinsic.fy = cv_params["Camera.fy"];
    params->cam_intrinsic.cx = cv_params["Camera.cx"];
    params->cam_intrinsic.cy = cv_params["Camera.cy"];
    params->cam_intrinsic.s  = cv_params["Camera.s"];

    params->cam_intrinsic.k1 = cv_params["Camera.k1"];
    params->cam_intrinsic.k2 = cv_params["Camera.k2"];
    params->cam_intrinsic.p1 = cv_params["Camera.p1"];
    params->cam_intrinsic.p2 = cv_params["Camera.p2"];
    params->cam_intrinsic.k3 = cv_params["Camera.k3"];
    params->cam_intrinsic.width = cv_params["Camera.width"];
    params->cam_intrinsic.height = cv_params["Camera.height"];

    // Load wheel intrinsic.读取车轮内参
    params->wheel_param.kl = cv_params["Wheel.kl"];
    params->wheel_param.kr = cv_params["Wheel.kr"];
    params->wheel_param.b = cv_params["Wheel.b"];
    params->wheel_param.noise_factor = cv_params["Wheel.noise_factor"];

    // Load extrinsic.  加载外参
    cv::Mat cv_O_R_C;
    cv_params["Extrinsic.O_R_C"] >> cv_O_R_C;  //camera->odometry的旋转
    cv::Mat cv_O_p_C;
    cv_params["Extrinsic.O_p_C"] >> cv_O_p_C;//camera->odometry的平移
    cv::Mat cv_C_p_Gps;
    cv_params["Extrinsic.C_p_Gps"] >> cv_C_p_Gps;//gps->camera的平移

    params->extrinsic.O_R_C << 
        cv_O_R_C.at<double>(0, 0), cv_O_R_C.at<double>(0, 1), cv_O_R_C.at<double>(0, 2), 
        cv_O_R_C.at<double>(1, 0), cv_O_R_C.at<double>(1, 1), cv_O_R_C.at<double>(1, 2), 
        cv_O_R_C.at<double>(2, 0), cv_O_R_C.at<double>(2, 1), cv_O_R_C.at<double>(2, 2), 
    params->extrinsic.O_p_C << cv_O_p_C.at<double>(0, 0), cv_O_p_C.at<double>(1, 0), cv_O_p_C.at<double>(2, 0);
    params->extrinsic.C_p_Gps << cv_C_p_Gps.at<double>(0, 0), cv_C_p_Gps.at<double>(1, 0), cv_C_p_Gps.at<double>(2, 0)