多传感器融合定位(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);

    // Visualization 可视化部分
    params->viz_config.cam_size = cv_params["viz_config.cam_size"];//相机大小
    params->viz_config.cam_line_width = cv_params["viz_config.cam_line_width"];
    params->viz_config.point_size = cv_params["viz_config.point_size"];
    params->viz_config.wheel_frame_size = cv_params["viz_config.wheel_frame_size"];
    params->viz_config.view_point_x = cv_params["viz_config.view_point_x"];
    params->viz_config.view_point_y = cv_params["viz_config.view_point_y"];
    params->viz_config.view_point_z = cv_params["viz_config.view_point_z"];
    params->viz_config.view_point_f = cv_params["viz_config.view_point_f"];
    params->viz_config.img_height = cv_params["viz_config.img_height"];//图像高度
    params->viz_config.img_width = cv_params["viz_config.img_width"];//图像宽度
    params->viz_config.max_traj_length = cv_params["viz_config.max_traj_length"];//最大轨迹长度
    params->viz_config.max_num_features = cv_params["viz_config.max_num_features"];//最大特征点
    params->viz_config.max_gps_length = cv_params["viz_config.max_gps_length"];//最大GPS长度
    params->viz_config.gps_point_size = cv_params["viz_config.gps_point_size"];
    cv_params["viz_config.show_raw_odom"] >> params->viz_config.show_raw_odom; //显示里程计
    cv_params["viz_config.show_gps_points"] >> params->viz_config.show_gps_points;//显示gps

    // Triangulator  三角化
    params->tri_config.max_proj_res = cv_params["tri_config.max_proj_res"];
    params->tri_config.min_dist = cv_params["tri_config.min_dist"];
    params->tri_config.max_dist = cv_params["tri_config.max_dist"];

    // Feature tracker  特征跟踪
    params->tracker_config.max_num_corners = cv_params["tracker_config.max_num_corners"];
    params->tracker_config.quality_level = cv_params["tracker_config.quality_level"];
    params->tracker_config.min_dist = cv_params["tracker_config.min_dist"];

    // Visual Updater   视觉更新
    params->visual_updater_config.visual_noise = cv_params["visual_updater.visual_noise"];
    params->visual_updater_config.min_window_length = cv_params["visual_updater.min_window_length"];
    params->visual_updater_config.min_cam_dist_to_triangulate = cv_params["visual_updater.min_cam_dist_to_triangulate"];
    params->visual_updater_config.min_res_size = cv_params["visual_updater.min_res_size"];
    
    // Plane Updater
    params->plane_updater_config.plane_rot_noise = cv_params["plane_updater.plane_rot_noise"];
    params->plane_updater_config.plane_trans_noise = cv_params["plane_updater.plane_trans_noise"];

    // System config.  系统设置
    params->sys_config.sliding_window_size = cv_params["sys_config.sliding_window_size"];
    cv_params["sys_config.compute_raw_odom"] >> params->sys_config.compute_raw_odom; //计算里程计
    cv_params["sys_config.enable_plane_update"] >> params->sys_config.enable_plane_update;
    cv_params["sys_config.enable_gps_update"] >> params->sys_config.enable_gps_update;
    
}

里程计图像时间同步类

WheelImageSynchronizer::WheelImageSynchronizer(const int max_wheel_buffer_length, 
                                               const int max_image_buffer_length)
    : max_wheel_buffer_length_(max_wheel_buffer_length), //最大里程计缓存长度
      max_image_buffer_length_(max_image_buffer_length),//最大图像缓存长度
      last_image_timestamp_(-1.) { }//上一帧时间戳

2、往系统中输入数据(image wheel gps)

2.1 图像数据

bool FilterFusionSystem::FeedImageData(const double timestamp, const cv::Mat& image) {
    // Convert to internal struct.
    const TGK::BaseType::MonoImageDataPtr img_ptr = std::make_shared<TGK::BaseType::MonoImageData>();
    img_ptr->timestamp = timestamp;
    img_ptr->image = image;

    // Sync with wheel data.
    data_sync_->FeedMonoImageData(img_ptr);
    return true;
}
void WheelImageSynchronizer::FeedMonoImageData(const BaseType::MonoImageDataConstPtr& image_data) {
    assert(image_data != nullptr);
    
    // Reject rolling back camera data.
     //image_buffer_是队列deque类型
    if (!image_buffer_.empty() && image_data->timestamp <= image_buffer_.back()->timestamp) {
        LOG(ERROR) << "[FeedMonoImageData]: Image timestamp rolling back!";
        return;
    }

    image_buffer_.push_back(image_data);
    //里程计缓存非空而且图像的时间戳小于车轮的时间戳
    while (image_buffer_.size() > max_image_buffer_length_ || 
           (!wheel_buffer_.empty() && image_buffer_.front()->timestamp < wheel_buffer_.front()->timestamp)) {
        image_buffer_.pop_front();
    }
}

2.2 里程计数据

bool FilterFusionSystem::FeedWheelData(const double timestamp, const double left, const double right) {
    // Convert to internal struct.
    const TGK::BaseType::WheelDataPtr wheel_ptr = std::make_shared<TGK::BaseType::WheelData>();
    wheel_ptr->timestamp = timestamp;
    wheel_ptr->left = left;
    wheel_ptr->right = right;

    // Sync with image data.
    std::vector<TGK::BaseType::WheelDataConstPtr> wheel_data_segment;
    TGK::BaseType::MonoImageDataConstPtr img_ptr;
    if (!data_sync_->FeedWheelData(wheel_ptr, &wheel_data_segment, &img_ptr)) { 
        return true;
    }

    // Initialize.
    if (!initialized_) {
        initializer_->Initialize(img_ptr->timestamp, &state_);

        // Adjust initialization for GPS Updater.
        if (config_.enable_gps_update) {
            if (latest_gps_data_ == nullptr || 
                std::abs(latest_gps_data_->timestamp - img_ptr->timestamp) > 0.5) {
                return true;
            }

            // Use bigger covariance.
            //初始化使用大的协方差
            state_.covariance.block<3, 3>(3, 3) = latest_gps_data_->cov;
            state_.covariance(2, 2) = 180. * 180. * kDeg2Rad * kDeg2Rad;

            // Set init lon lat heig for gps updater.
            //设置初始的经度、纬度以及海拔
            gps_updater_->SetInitLonLatHei(latest_gps_data_->lon_lat_hei);
        } // For gps initialization.

        initialized_ = true;
        return true;
    }

    // Propagate state.
    //状态传播
    for (size_t i = 1; i < wheel_data_segment.size(); ++i) {
        const auto begin_wheel = wheel_data_segment[i-1];
        const auto end_wheel = wheel_data_segment[i];

        // Check timestamp.
        assert(std::abs(begin_wheel->timestamp - state_.timestamp) < 1e-6);

        propagator_->Propagate(begin_wheel->left, begin_wheel->right,
                               end_wheel->left, end_wheel->right,
                               &state_);

        // Set timestamp.
        state_.timestamp = end_wheel->timestamp;
        
        // Compute raw wheel odometry for comparison with FilterFusion.
        //计算原始里程计与滤波算法作对比,输出位姿
        if (wheel_propagator_  != nullptr) {
            wheel_propagator_->PropagateUsingEncoder(begin_wheel->left, begin_wheel->right, 
                                                     end_wheel->left, end_wheel->right, 
                                                     &odom_G_R_O_, &odom_G_p_O_);
        }
    }

    /// 1. Visual update 视觉更新
    // Augment state / Clone new camera state. 状态增强 
    AugmentState(img_ptr->timestamp, (++kFrameId), &state_);

    // Track features.  跟踪特征
    const auto image_type = img_ptr->type;
    std::vector<Eigen::Vector2d> tracked_pts; 
    std::vector<long int> tracked_pt_ids;
    std::vector<long int> lost_pt_ids;
    std::set<long int> new_pt_ids;
    //图像类型为单目
    if (image_type == TGK::BaseType::MeasureType::kMonoImage) {
        feature_tracker_->TrackImage(img_ptr->image, &tracked_pts, &tracked_pt_ids, &lost_pt_ids, &new_pt_ids);
    } else if (image_type == TGK::BaseType::MeasureType::kSimMonoImage) {
        const TGK::BaseType::SimMonoImageDataConstPtr sim_img_ptr 
            = std::dynamic_pointer_cast<const TGK::BaseType::SimMonoImageData>(img_ptr);
        sim_feature_tracker_->TrackSimFrame(sim_img_ptr->features, sim_img_ptr->feature_ids, 
                                            &tracked_pts, &tracked_pt_ids, &lost_pt_ids, &new_pt_ids);
    } else {
        LOG(ERROR) << "Not surpport image type.";
        exit(EXIT_FAILURE);
    }

    // Do not marginalize the last state if no enough camera state in the buffer.
    //缓存中没有足够的相机状态则不进行边缘化
    const bool marg_old_state = state_.camera_frames.size() >= config_.sliding_window_size_;

    // Update state. 更新状态
    std::vector<Eigen::Vector2d> tracked_features;
    std::vector<Eigen::Vector2d> new_features;
    std::vector<Eigen::Vector3d> map_points;
    visual_updater_->UpdateState(img_ptr->image, marg_old_state, 
                                 tracked_pts, tracked_pt_ids, lost_pt_ids, new_pt_ids,
                                 &state_, &tracked_features, &new_features, &map_points);

    // Marginalize old state.
    if (marg_old_state) { MargOldestState(&state_); }

    /// 2. Plane Update.
    if (config_.enable_plane_update && plane_updater_ != nullptr) {
        plane_updater_->UpdateState(&state_);
    } 

    /// Visualize.
    viz_->DrawWheelPose(state_.wheel_pose.G_R_O, state_.wheel_pose.G_p_O);
    viz_->DrawFeatures(map_points);
    viz_->DrawCameras(GetCameraPoses());

    if (image_type == TGK::BaseType::MeasureType::kMonoImage) {
        viz_->DrawImage(img_ptr->image, tracked_features, new_features);
    } else if (image_type == TGK::BaseType::MeasureType::kSimMonoImage) {
        viz_->DrawColorImage(img_ptr->image);
    }

    // Draw raw wheel odometry.
    if (config_.compute_raw_odom_) {
        viz_->DrawWheelOdom(odom_G_R_O_, odom_G_p_O_);
    }

    return true;
}

时间同步下的输入里程计数据

 bool WheelImageSynchronizer::FeedWheelData(const BaseType::WheelDataConstPtr& wheel_data, 
                                           std::vector<BaseType::WheelDataConstPtr>* wheel_data_segment,
                                           BaseType::MonoImageDataConstPtr* mono_image_data) {
    assert(wheel_data != nullptr);
    assert(wheel_data_segment != nullptr);
    assert(mono_image_data != nullptr);

    // Reject rolling back wheel data.
    if (!wheel_buffer_.empty() && wheel_data->timestamp <= wheel_buffer_.back()->timestamp) {
        LOG(ERROR) << "[FeedWheelData]: Wheel timestamp rolling back!";
        return false;
    }

    // First push data to deque.
    wheel_buffer_.push_back(wheel_data);
    if (wheel_buffer_.size() > max_wheel_buffer_length_) {
        wheel_buffer_.pop_front();
    }

    if (image_buffer_.empty()) {
        return false;
    }

    // Then check if the wheel data timestamp is bigger than the image timestamp.
     //检查里程计的时间戳是否大于图像的时间戳
    const double image_timestamp = image_buffer_.front()->timestamp;
    if (wheel_data->timestamp < image_timestamp) {
        return false;
    }

    *mono_image_data = image_buffer_.front();
    image_buffer_.pop_front();

    // Collect all wheel data before the first image.
     //在第一张图像之前收集里程计数据
    const double last_used_wheel_timestamp = (last_image_timestamp_ > 0.) ? 
                                             last_image_timestamp_ : 
                                             wheel_buffer_.front()->timestamp; 

    // Collect Wheel data between the last timestamp and the current image time.
     //收集上一帧图像和当前帧图像之间的里程计数据
    if (!CollectWheelDataBetweenTimes(wheel_buffer_, last_used_wheel_timestamp, 
                                      image_timestamp, wheel_data_segment)) {
        LOG(ERROR) << "[FeedWheelData]: Failed to collect Wheel data between times!";
        return false;
    }

    last_image_timestamp_ = image_timestamp;

    return true;
}

收集上一帧图像和当前帧图像之间的里程计数据

max_wheel_buffer_length :1000帧数据

max_image_buffer_length :10帧数据

inline bool CollectWheelDataBetweenTimes(const std::deque<BaseType::WheelDataConstPtr>& wheel_buffer, 
                                       const double start_time, const double end_time, 
                                       std::vector<BaseType::WheelDataConstPtr>* wheels) {
    assert(wheels != nullptr);

    if (wheel_buffer.empty()) {
        LOG(ERROR) << "[CollectWheelDataBetweenTimes]: Wheel buffer is empty!";
        return false;
    }

    if (start_time < wheel_buffer.front()->timestamp || end_time > wheel_buffer.back()->timestamp) {
        LOG(ERROR) << "[CollectWheelDataBetweenTimes]: Search timestamp is out of the Wheel buffer!";
        return false;
    }

    // Check timestamp.
    const double time_interval = end_time - start_time;
    // TODO: make it to be configs.
    constexpr double kMinWheelTimeInterval = 0.0001;
    if (time_interval < kMinWheelTimeInterval) {
        LOG(ERROR) << "[CollectWheelDataBetweenTimes]: The interval between start and end time is too small!";
        return false;
    }

    // Get end_idx.   末尾索引
    int end_idx = wheel_buffer.size() - 1;
    while (wheel_buffer[end_idx]->timestamp >= end_time) {
        --end_idx;
    }

    // Get start idx.  起始索引
    int start_idx = end_idx;
    while (wheel_buffer[start_idx]->timestamp > start_time) {
        --start_idx;
    }

    // Interpolate the first data.  插值第一帧数据
    BaseType::WheelData start_wheel;
    if (!InterpolateWheelData(*(wheel_buffer[start_idx]), *(wheel_buffer[start_idx + 1]), start_time, &start_wheel)) {
        LOG(ERROR) << "[CollectWheelDataBetweenTimes]: Failed to interpolate the start Wheel data!";
        return false;
    }
    wheels->push_back(std::make_shared<BaseType::WheelData>(start_wheel));
    
    for (size_t i = start_idx + 1; i <= end_idx; ++i) {
        wheels->push_back(wheel_buffer[i]);
    }
    //插值end_time时间戳的数据
    BaseType::WheelData end_wheel;
    if (!InterpolateWheelData(*(wheel_buffer[end_idx]), *(wheel_buffer[end_idx + 1]), end_time, &end_wheel)) {
        LOG(ERROR) << "[CollectWheelDataBetweenTimes]: Failed to interplate the end Wheel data!";
    }
    wheels->push_back(std::make_shared<BaseType::WheelData>(end_wheel));

    return true;
}

里程计数据的插值

inline bool InterpolateWheelData(const BaseType::WheelData& prev_wheel, 
                                 const BaseType::WheelData& next_wheel,
                                 const double inter_timestamp,
                                 BaseType::WheelData* inter_wheel) {
    assert(inter_wheel != nullptr);

    const double time_interval = next_wheel.timestamp - prev_wheel.timestamp;
    if (time_interval < 1e-6) {
        LOG(ERROR) << "[InterpolateWheelData]: Time interval between two wheel data is too small!";
        return false;
    }

    if (inter_timestamp < prev_wheel.timestamp || inter_timestamp > next_wheel.timestamp) {
        LOG(ERROR) << "[InterpolateWheelData]: The interpolate timestamp is not in the interval of the two Wheel data.";
        return false;
    }
    //数据的插值
    const double ratio = (inter_timestamp - prev_wheel.timestamp) / time_interval;
    const double one_minus_ratio = 1. - ratio;
    inter_wheel->timestamp = inter_timestamp;
    inter_wheel->left = ratio * next_wheel.left + one_minus_ratio * prev_wheel.left;
    inter_wheel->right = ratio * next_wheel.right + one_minus_ratio * prev_wheel.right;

    return true;
}
//构造函数  里程计传播初始化
Propagator::Propagator(const double kl, const double kr,const double b, const double noise_factor) {
    wheel_propagator_ = std::make_unique<TGK::WheelProcessor::WheelPropagator>(kl, kr, b, noise_factor);
}
// const double begin_wl(起始轮左轮), const double begin_wr,(起始轮右轮)
//  const double end_wl(结束轮左轮), const double end_wr,(结束轮右轮)
//  State* state (状态)
void Propagator::Propagate(const double begin_wl, const double begin_wr,
                           const double end_wl, const double end_wr,
                           State* state) {
    // Propagate mean and covariance of the wheel pose state.
    Eigen::Matrix<double, 6, 6> Phi;
    //车轮位姿的协方差
    Eigen::Matrix<double, 6, 6> wheel_pose_cov = state->covariance.topLeftCorner<6, 6>();
    wheel_propagator_->PropagateUsingEncoder(begin_wl, begin_wr, end_wl, end_wr, 
                                             &state->wheel_pose.G_R_O, &state->wheel_pose.G_p_O,
                                             &Phi, 
                                             &wheel_pose_cov);
    state->covariance.topLeftCorner<6, 6>() = wheel_pose_cov;
    
    // Propagate covariance of other states.
    const int cov_size = state->covariance.rows();
    const int other_size = cov_size - 6;
    if (other_size <= 0) { return; }

    state->covariance.block(0, 6, 6, other_size) = 
        Phi * state->covariance.block(0, 6, 6, other_size).eval();

    //Force symmetric.
    state->covariance = state->covariance.eval().selfadjointView<Eigen::Upper>();
}

使用编码器传播

void WheelPropagator::PropagateUsingEncoder(const double begin_wl, const double begin_wr,
                                            const double end_wl, const double end_wr,
                                            Eigen::Matrix3d* G_R_O, Eigen::Vector3d* G_p_O,
                                            Eigen::Matrix<double, 6, 6>* J_wrt_pose,
                                            Eigen::Matrix<double, 6, 6>* cov) {
    // Pre-computation.
    const double left_dist = (end_wl - begin_wl) * kl_;
    const double right_dist = (end_wr - begin_wr) * kr_;
    const double delta_yaw = (right_dist - left_dist) / b_;
    const double delta_dist = (right_dist + left_dist) * 0.5;

    // Mean.
    const Eigen::Matrix3d delta_R = Eigen::AngleAxisd(delta_yaw, Eigen::Vector3d::UnitZ()).toRotationMatrix();
    const Eigen::Vector3d delta_p = Eigen::Vector3d(delta_dist, 0., 0.);
    const Eigen::Matrix3d old_G_R_O = *G_R_O;

    *G_R_O = old_G_R_O * delta_R;
    *G_p_O = *G_p_O + old_G_R_O * delta_p;

    // Jacobian or Phi.
    if (J_wrt_pose != nullptr) {
        *J_wrt_pose << delta_R.transpose(),            Eigen::Matrix3d::Zero(), 
                      -old_G_R_O *Util::Skew(delta_p), Eigen::Matrix3d::Identity();
    }

    // Covariance.
    if (cov != nullptr && J_wrt_pose != nullptr) {
        Eigen::Matrix<double, 6, 6> noise = Eigen::Matrix<double, 6, 6>::Zero();
        noise(0, 0) = roll_pitch_noise_;
        noise(1, 1) = roll_pitch_noise_;
        noise(2, 2) = delta_yaw * delta_yaw * noise_factor_ * noise_factor_;
        noise(3, 3) = delta_dist * delta_dist * noise_factor_ * noise_factor_;
        noise(4, 4) = delta_dist * delta_dist * noise_factor_ * noise_factor_;
        noise(5, 5) = z_noise_;

        Eigen::Matrix<double, 6, 6> J_wrt_delta_pose = Eigen::Matrix<double, 6, 6>::Identity();
        J_wrt_delta_pose.block<3, 3>(3, 3) = old_G_R_O;

        *cov = (*J_wrt_pose) * cov->eval() * J_wrt_pose->transpose() + 
               J_wrt_delta_pose * noise * J_wrt_delta_pose.transpose();
    }
}

增强状态

/// Causion: This function does not process frame id 状态增强
void AugmentState(const double timestamp, const long int frame_id, State* state) {
    const CameraFramePtr cam_frame = std::make_shared<CameraFrame>();
    cam_frame->timestamp = timestamp;
    cam_frame->id = frame_id;
    
    // Compute mean.  计算均值
    cam_frame->G_R_C = state->wheel_pose.G_R_O * state->extrinsic.O_R_C;
    cam_frame->G_p_C = state->wheel_pose.G_p_O + state->wheel_pose.G_R_O * state->extrinsic.O_p_C;

    // Set index.  设置索引
    cam_frame->state_idx = state->covariance.rows();

    // Push to state vector.
    state->camera_frames.push_back(cam_frame);

    // Extend covaraicne.  扩展协方差
    const int old_size = state->covariance.rows();
    const int new_size = old_size + 6;
    state->covariance.conservativeResize(new_size, new_size);
    state->covariance.block(old_size, 0, 6, new_size).setZero();
    state->covariance.block(0, old_size, new_size, 6).setZero();

    /// Compute covariance. 计算协方差
    Eigen::Matrix<double, 6, 6> J_wrt_wheel_pose;
    J_wrt_wheel_pose << state->extrinsic.O_R_C.transpose(), Eigen::Matrix3d::Zero(),
                        -state->wheel_pose.G_R_O * TGK::Util::Skew(state->extrinsic.O_p_C), 
                        Eigen::Matrix3d::Identity();

    const Eigen::Matrix<double, 6, 6> cov11 = state->covariance.block<6, 6>(0, 0);
    state->covariance.block<6, 6>(old_size, old_size) = J_wrt_wheel_pose * cov11 * J_wrt_wheel_pose.transpose();
     
    const auto& cov_top_rows = state->covariance.block(0, 0, 6, old_size);
    // New lower line.
    state->covariance.block(old_size, 0, 6, old_size) = J_wrt_wheel_pose * cov_top_rows;

    // Force symmetric.  力对称
    state->covariance = state->covariance.eval().selfadjointView<Eigen::Lower>();
}

关于C++中问题的疑问?

智能指针

std::make_unique(C++14)作用?

std::make_shared

容器中的成员函数:

back():最后一个元素的引用

pop_front():删除第一个元素

front():起始元素的引用

上一篇:成功解决python下载numpy报错。各种错,各种飘红,原来这么简单就改好了。。


下一篇:ansible从入门到精通 从安装到使用