多传感器融合定位（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, ¶m_);
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():起始元素的引用