gici-open 项目地址：
https://github.com/chichengcn/gici-open
在multisensor_estimating.cpp 文件中的processEstimator()函数 正式开启多传感器Estimating线程的后端估计部分。本文主要介绍rtk/ins/camera紧组合的后端优化部分。在后端估计部分涉及大量的GNSS相关算法(如双差计算，周条探测，模糊度估计，对流层电离层改正等内容)，由于此项目是以组合导航为主题，我更加关注多传感器的融合的流程及参数估计。不会详细分析以上GNSS相关算法，建议在专业的GNSS软件中去学习，如本项目的RTKLIB库。或者前往我的RTKLIB学习专栏，及其他博主处详细学习。
// Process estimator
bool MultiSensorEstimating::processEstimator()
{
// Check if we have data to process检测有没有要处理的数据
mutex_input_.lock();
if (measurements_.size() == 0) {
}// 从 measurements_ 观测值队列取出并删除最前面的数据
EstimatorDataCluster measurement = measurements_.front();
measurements_.pop_front();
mutex_input_.unlock();
// Check pending如果measurements_观测量队列数据超过5，且和之前的数量不一致就报警告
if (measurements_.size() > 5) {
if (last_backend_pending_num_ != measurements_.size()) {
LOG(WARNING) << "Large backend pending: " << measurements_.size()
<< " measurements are waiting!";
}
last_backend_pending_num_ = measurements_.size();
}
// Set coordinate and gravity// 如果还没有 solution_.coordinate
if (solution_.coordinate == nullptr)
{
Eigen::Vector3d position_ecef;
// Force set coordinate zero手动设置初始坐标
if (force_initial_global_position_) {
GeoCoordinate coordinate;
position_ecef = coordinate.convert(
GeoCoordinate::degToRad(initial_global_position_),
GeoType::LLA, GeoType::ECEF);
}
// get coordinate zero from GNSS raw根据原始观测值求出 SPP 解作为初始坐标
else if (measurement.gnss &&
estimatorTypeContains(SensorType::GNSS, type_)) {
if (!spp_estimator_->addMeasurement(measurement)) {
return false;
}
if (!spp_estimator_->estimate()) {
return false;
}
position_ecef = spp_estimator_->getPositionEstimate();
measurement.gnss->position = position_ecef;
}
// get coordinate zero from solution
else if (measurement.solution) {
position_ecef = measurement.solution->coordinate->convert(
measurement.solution->pose.getPosition(),
GeoType::ENU, GeoType::ECEF);
}
// no where to get coordinate zero
else {
return false;
}
// set coordinate
solution_.coordinate = std::make_shared<GeoCoordinate>(
position_ecef, GeoType::ECEF);
Eigen::Vector3d lla = solution_.coordinate->convert(
position_ecef, GeoType::ECEF, GeoType::LLA);
estimator_->setCoordinate(solution_.coordinate);
// Set gravity if needed
if (estimatorTypeContains(SensorType::IMU, type_)) {
double gravity = earthGravity(lla);
std::shared_ptr<ImuEstimatorBase> imu_estimator =
std::dynamic_pointer_cast<ImuEstimatorBase>(estimator_);
CHECK_NOTNULL(imu_estimator);
imu_estimator->setGravity(gravity);
}
}
// Process estimator正式估计
bool is_updated = false;
// add measurement
if (estimator_->addMeasurement(measurement)) {
// solve 如果估计有效，将 is_updated 标志设置为 true
if (estimator_->estimate()) is_updated = true;//std::shared_ptr<EstimatorBase> estimator_;在estimating.h文件中
// check if estimator valid
if (estimator_->getStatus() == EstimatorStatus::Diverged) {
// reset estimator 如果估计发散，重置估计器并将 is_updated 标志设置为 false
LOG(WARNING) << "Reset estimator because it is diverge!";
resetProcessors();
is_updated = false;
}
// log intermediate data
estimator_->logIntermediateData();
}
// Backend updated更新结果和初始状态
if (is_updated)//估计有效
{
// update flag
backend_firstly_updated_ = true;
// align timeline for output control如果测量值的时间标签与预期的对齐标签一致，添加时间戳以进行输出控制
if (output_align_tag_ == measurement.tag) {
mutex_output_.lock();
if (checkDownsampling(measurement.tag)) {
output_timestamps_.push_back(measurement.timestamp);
}
mutex_output_.unlock();
}
// check pendding 如果队列中还有待处理的测量值，计算最后两个时间戳之间的时间差，并可以在此处添加反馈
if (measurements_.size() > 1) {
double pending_period = measurements_.back().timestamp -
solution_.timestamp;
// add feedbacks here
}
}
return true;
}
其中
// add measurement
if (estimator_->addMeasurement(measurement)) {
// solve 如果估计有效，将 is_updated 标志设置为 true
if (estimator_->estimate()) is_updated = true;//std::shared_ptr<EstimatorBase> estimator_;在estimating.h文件中
// check if estimator valid
if (estimator_->getStatus() == EstimatorStatus::Diverged) {
// reset estimator 如果估计发散，重置估计器并将 is_updated 标志设置为 false
LOG(WARNING) << "Reset estimator because it is diverge!";
resetProcessors();
is_updated = false;
}
通过estimator_-> 分别进入添加量测，参数估计，验证有效性流程。
在estimating.h文件中的class EstimatingBase可以找到如下：
std::shared_ptr<EstimatorBase> estimator_;
estimator_-> 为指向EstimatorBase类的共享指针。
在rtk_imu_camera_rrr_estimator.h文件中得到三个父类：
class RtkImuCameraRrrEstimator :
public GnssEstimatorBase,
public VisualEstimatorBase,
public ImuEstimatorBase
因此estimator_-> 是通过继承父类的指针中的虚函数从而调用子类
**class RtkImuCameraRrrEstimator : **中的成员函数。
// GNSS/IMU initialization
if (coordinate_ == nullptr || !gravity_setted_) return false;//没有设置初始坐标和重力
if (!gnss_imu_initializer_->finished()) {//gnss_imu未完成初始化
if (gnss_imu_initializer_->getCoordinate() == nullptr) {//检查gnss_imu坐标是否设置
gnss_imu_initializer_->setCoordinate(coordinate_);//设置坐标
initializer_sub_estimator_->setCoordinate(coordinate_);
gnss_imu_initializer_->setGravity(imu_base_options_.imu_parameters.g);//获取重力加速度
}
if (gnss_imu_initializer_->addMeasurement(measurement)) {//gnss_imu坐标设置了
gnss_imu_initializer_->estimate();//初始化估计
// set result to estimator
setInitializationResult(gnss_imu_initializer_);
}
return false;
}
其中的gnss_imu_initializer_ 为指向类GnssImuInitializer 的共享指针，具体可在gnss_imu_initializer.h文件中查看。
// Add GNSS
if (measurement.gnss) {//GNSS数据存在时
// feed to covariance estimator将GNSS数据添加到RTK估计器中
if (!ambiguity_covariance_coordinate_setted_ && coordinate_) {//没有设置坐标
ambiguity_covariance_estimator_->setCoordinate(coordinate_);//设置坐标
ambiguity_covariance_coordinate_setted_ = true;//设置完成
}
if (coordinate_ && ambiguity_covariance_estimator_->addMeasurement(measurement)) {//添加GNSS数据
ambiguity_covariance_estimator_->estimate();//估计
}
// feed to local
GnssMeasurement rov, ref;
meausrement_align_.add(measurement);//把基准站、流动站数据放到不同的变量里
if (meausrement_align_.get(rtk_options_.max_age, rov, ref)) {//把基准站、流动站数据对应起来
return addGnssMeasurementAndState(rov, ref);//向因子图中加 RTK 残差因子
}
}
// Add images添加图像
if (measurement.frame_bundle) {//如果视觉没初始化过，调用 visualInitialization()初始化
if (!visual_initialized_) return visualInitialization(measurement.frame_bundle);
return addImageMeasurementAndState(measurement.frame_bundle);
}
在gnss_imu_initializer.cpp文件中找到具体代码：
// Add measurement
bool GnssImuInitializer::addMeasurement(const EstimatorDataCluster& measurement)
{
if (finished_) return false;//初始坐标和重力没设置
// Add IMU添加IMU量测值
if (measurement.imu && measurement.imu_role == ImuRole::Major) {
addImuMeasurement(*measurement.imu);//添加IMU量测值
}
// Add GNSS by solution measurement通过解决方案测量添加GNSS
if (measurement.solution &&
(measurement.solution_role == SolutionRole::Position ||
measurement.solution_role == SolutionRole::Velocity ||
measurement.solution_role == SolutionRole::PositionAndVelocity)) {
GnssSolution gnss_solution = convertSolutionToGnssSolution(
*measurement.solution, measurement.solution_role);
return addGnssSolutionMeasurement(gnss_solution);
}
// Add GNSS by raw measurement通过原始测量添加GNSS
if (sub_gnss_estimator_ && measurement.gnss) {
if (sub_gnss_estimator_->addMeasurement(*measurement.gnss)) {
if (sub_gnss_estimator_->estimate()) {
Solution solution;// 获取GNSS估计的解
solution.coordinate = coordinate_;// 获取坐标
solution.timestamp = sub_gnss_estimator_->getTimestamp();/
solution.pose = sub_gnss_estimator_->getPoseEstimate();
solution.speed_and_bias = sub_gnss_estimator_->getSpeedAndBiasEstimate();
solution.covariance = sub_gnss_estimator_->getCovariance();// 获取协方差
// 获取GNSS估计的状态信息
std::shared_ptr<GnssEstimatorBase> gnss_estimator =
std::dynamic_pointer_cast<GnssEstimatorBase>(sub_gnss_estimator_);// 获取GNSS估计器
solution.status = gnss_estimator->getSolutionStatus();// 状态信息
solution.num_satellites = gnss_estimator->getNumberSatellite();// 卫星数量
solution.differential_age = gnss_estimator->getDifferentialAge();// 差分时间
SolutionRole role;// 是否将速度纳入到解中
if (gnss_estimator->hasVelocityEstimate()) {
role = SolutionRole::PositionAndVelocity;
}
else {
role = SolutionRole::Position;
}
// 将解转换为GNSS解，并添加到初始化中
GnssSolution gnss_solution = convertSolutionToGnssSolution(solution, role);
return addGnssSolutionMeasurement(gnss_solution);//添加GNSS解
}
}
}
return false;
}
// Apply initialization
bool GnssImuInitializer::estimate()
{
optimize();//调用optimize函数执行初始化的优化过程
// Log information// 记录信息
if (base_options_.verbose_output) {
LOG(INFO) << "GNSS/IMU initialization: "
<< "Iterations: " << graph_->summary.iterations.size() << ", "
<< std::scientific << std::setprecision(3)
<< "Initial cost: " << graph_->summary.initial_cost << ", "
<< "Final cost: " << graph_->summary.final_cost;
}
finished_ = true;// 标记初始化过程已完成
return true;
}
找到GnssImuInitializer 的父类MultisensorInitializerBase
// Estimator
class GnssImuInitializer :
public MultisensorInitializerBase,
public GnssLooseEstimatorBase,
public ImuEstimatorBase
而类MultisensorInitializerBase 又继承自类EstimatorBase 并找到如下：
protected:
// Apply ceres optimization
virtual void optimize();
最终在estimator_base.cpp文件中找到如下实现：
// Apply ceres optimization设置Ceres Solver的优化选项，调用求解器进行优化，并根据需要更新协方差。
void EstimatorBase::optimize()
{
graph_->options.linear_solver_type = base_options_.solver_type;//设置线性求解器的类型
graph_->options.trust_region_strategy_type = base_options_.trust_region_strategy_type;//设置信任域策略的类型
graph_->options.num_threads = base_options_.num_threads;//设置用于优化的线程数
graph_->options.max_num_iterations = base_options_.max_iteration;//设置最大迭代次数
#ifdef NDEBUG //如果在编译时定义了 NDEBUG 宏，那么会启用一些调试设置。
graph_->options.max_solver_time_in_seconds = base_options_.max_solver_time;
#endif
if (base_options_.verbose_output) {
// graph_->options.minimizer_progress_to_stdout = true;
}
else {//设置日志类型为 SILENT（静默），并禁止向 stdout 打印优化器进度。
graph_->options.logging_type = ceres::LoggingType::SILENT;
graph_->options.minimizer_progress_to_stdout = false;
}
// call solver
graph_->solve();//调用求解器进行优化
// update covariance
if (base_options_.compute_covariance && can_compute_covariance_) {
updateCovariance(latestState());//更新协方差
}
}
其中的核心graph_ 为指向类Graph 的智能指针，此处略过，后期在详细介绍。
// Set initializatin result
void RtkImuCameraRrrEstimator::setInitializationResult(
const std::shared_ptr<MultisensorInitializerBase>& initializer)
{
CHECK(initializer->finished());//检查初始化器是否已经完成
// Cast to desired initializer 将传入的MultisensorInitializerBase类型初始化器强制转换为GnssImuInitializer类型
std::shared_ptr<GnssImuInitializer> gnss_imu_initializer =
std::static_pointer_cast<GnssImuInitializer>(initializer);
CHECK_NOTNULL(gnss_imu_initializer);//检查是否成功转换
// Arrange to window length
ImuMeasurements imu_measurements;
std::deque<GnssSolution> gnss_measurement_temp; // we do not use this
gnss_imu_initializer->arrangeToEstimator(//排列滑动窗口观测值，如果需要进行边缘化
rrr_options_.max_gnss_window_length_minor, marginalization_error_, states_,
marginalization_residual_id_, gnss_extrinsics_id_,
gnss_measurement_temp, imu_measurements);
for (auto it = imu_measurements.rbegin(); it != imu_measurements.rend(); it++) {//保持时间顺序
imu_mutex_.lock();//将整理好的IMU测量逆序遍历，并依次放入类成员变量imu_measurements_的前端
imu_measurements_.push_front(*it);
imu_mutex_.unlock();
}
// Shift memory for states and measurements调整内存以存储状态和测量
states_.push_back(State());//添加新的状态State
gnss_measurement_pairs_.resize(states_.size());//调整大小以匹配新的状态数量
ambiguity_states_.resize(states_.size());
frame_bundles_.push_back(nullptr);//添加空指针
}
//将GNSS和IMU初始化结果整理为估计器可以使用的形式
// Arrange the initialization results to estimator
bool GnssImuInitializer::arrangeToEstimator(const int window_length,
const std::shared_ptr<MarginalizationError>& marginalization_error,
std::deque<State>& states,
ceres::ResidualBlockId& marginalization_residual_id,
BackendId& gnss_extrinsics_id,
std::deque<GnssSolution>& gnss_solution_measurements,
ImuMeasurements& imu_measurements)
{
if (!finished_) return false;//检查初始化是否完成
// check if we need apply marginalization检查是否需要应用边缘化
if (states_.size() < window_length) {//如果历史状态数量小于指定的窗口长度
states = states_;//直接将历史状态、
marginalization_residual_id = marginalization_residual_id_;//边缘化残差ID、
gnss_extrinsics_id = gnss_extrinsics_id_;//GNSS外参ID、
gnss_solution_measurements = gnss_solution_measurements_;//GNSS解测量、
imu_measurements = imu_measurements_;//IMU测量传递给估计器，避免了边缘化的计算
return true;
}
// Check if marginalization input valid检查边缘化输入的有效性
CHECK_NOTNULL(marginalization_error);
CHECK(marginalization_residual_id == nullptr);//确保边缘化误差非空，而边缘化残差ID为空
marginalization_error_ = marginalization_error;
// Fill marginalization terms填充边缘化项
states.clear();//清空状态队列
std::deque<State>::reverse_iterator it_state = states_.rbegin();
for (int size = 0; it_state != states_.rend(); it_state++, size++)
{//状态队列的反向迭代器开始遍历每个状态
State& state = *it_state;
if (size < window_length - 1) {//如果状态索引小于窗口长度减1，则将状态直接添加到states中
states.push_front(state);//添加到状态队列的前端
continue;
}
//否则调用将该状态添加到边缘化残差块
// pose and speed and bias parameter and corresponding residuals
addImuStateMarginBlockWithResiduals(state);//为姿态、速度、偏置参数添加边缘化块和对应的残差
}
// Apply marginalization and add the item into graph
applyMarginalization();//应用边缘化操作
marginalization_residual_id = marginalization_residual_id_;//更新边缘化残差ID、
gnss_extrinsics_id = gnss_extrinsics_id_;//GNSS外参ID、
gnss_solution_measurements = gnss_solution_measurements_;//GNSS解测量
imu_measurements = imu_measurements_;//IMU测量
return true;
}
检查是否有遗漏的残差块:
for (auto id : marginalization_parameter_ids_) {
auto residuals = graph_->residuals(id.asInteger());
for (auto residual : residuals) {
marginalization_error_->addResidualBlock(residual.residual_block_id);
}
}
对于每个边缘化的参数块，检查图中是否有与之相关的残差块。如果有，将这些残差块添加到边缘化误差对象marginalization_error_ 中。
应用边缘化:
std::vector<uint64_t> parameter_blocks_to_be_marginalized;
for (auto margin_id : marginalization_parameter_ids_) {
parameter_blocks_to_be_marginalized.push_back(margin_id.asInteger());
}
marginalization_error_->marginalizeOut(parameter_blocks_to_be_marginalized,
marginalization_keep_parameter_blocks_);
将指定的参数块边缘化，marginalization_keep_parameter_blocks_参数指示哪些参数块应该保留。
更新错误计算:
if(parameter_blocks_to_be_marginalized.size() > 0) {
marginalization_error_->updateErrorComputation();
}
如果有要边缘化的参数块，更新计算误差。
再次添加边缘化项:
if(marginalization_error_->num_residuals()==0)
{
marginalization_error_.reset();
}
if (marginalization_error_)
{
std::vector<std::shared_ptr<ParameterBlock> > parameter_block_ptrs;
marginalization_error_->getParameterBlockPtrs(parameter_block_ptrs);
marginalization_residual_id_ = graph_->addResidualBlock(
marginalization_error_, nullptr, parameter_block_ptrs);
CHECK(marginalization_residual_id_)
<< "could not add marginalization error";
if (!marginalization_residual_id_) return false;
}
num_residuals()==0），则将其重置为nullptr。最终，函数返回true表示边缘化成功应用。
在imu_estimator_base.cpp文件中找到RtkImuCameraRrrEstimator 的父类ImuEstimatorBase 并找到下面代码：
时间戳检查:
if (imu_measurements_.size() != 0 &&
imu_measurements_.back().timestamp > imu_measurement.timestamp) {
LOG(WARNING) << "Received IMU with previous timestamp!";
}
这部分代码检查传入的IMU测量是否具有比imu_measurements_容器中最后一次记录的IMU测量更早的时间戳。如果是，将生成一个警告日志，指示收到了一个具有先前时间戳的IMU测量。
处理并存储IMU测量:
else {
imu_mutex_.lock();
imu_measurements_.push_back(imu_measurement);
imu_measurements_.back().angular_velocity =
rotateImuToBody(imu_measurements_.back().angular_velocity);
imu_measurements_.back().linear_acceleration =
rotateImuToBody(imu_measurements_.back().linear_acceleration);
imu_mutex_.unlock();
}
如果时间戳检查通过，代码将进入此块。它锁定imu_mutex_（可能是为了防止对IMU测量数据结构的并发访问），将新的IMU测量添加到imu_measurements_，然后使用rotateImuToBody函数将角速度和线性加速度测量从IMU坐标系旋转到载体坐标系。最后，解锁。
删除已使用的IMU测量:
imu_mutex_.lock();
if (states_.size() > 0 && !do_not_remove_imu_measurements_)
while (imu_measurements_.front().timestamp < oldestState().timestamp - 1.0) {
imu_measurements_.pop_front();
}
imu_mutex_.unlock();
在添加新的IMU测量后，代码再次锁定imu_mutex_。然后检查是否已记录状态，并且如果do_not_remove_imu_measurements_标志未设置。在循环内，它从imu_measurements_容器的前面删除IMU测量，直到前面测量的时间戳早于最早记录的状态的时间戳减去1.0。
在RtkImuCameraRrrEstimator类中找到：
std::unique_ptr ambiguity_covariance_estimator_ 前往rtk_estimator.cpp文件。
// Add measurement
bool RtkEstimator::addMeasurement(const EstimatorDataCluster& measurement)
{
GnssMeasurement rov, ref;
meausrement_align_.add(measurement);//将测量数据添加到测量对齐器
if (meausrement_align_.get(rtk_options_.max_age, rov, ref)) {//rtk_options_.max_age指定了允许的最大时间差，从测量对齐器中获取对齐后的GNSS测量
return addGnssMeasurementAndState(rov, ref);//将测量和相关的状态添加到估计器中
}
return false;
}
在rtl_estimator.h文件中找到：DifferentialMeasurementsAlign meausrement_align_
// Set measurements
inline void add(const EstimatorDataCluster& measurement) {
if (measurement.gnss && measurement.gnss_role == GnssRole::Rover) {
measurement_rov_.push_back(*measurement.gnss);//将测量值添加到流动站观测值队列中
}
if (measurement.gnss && measurement.gnss_role == GnssRole::Reference) {
measurement_ref_.push_back(*measurement.gnss);将测量值添加到参考站观测值队列中
}
}
// Get aligned从两个测量队列中获取对齐测量
inline bool get(const double max_age,
GnssMeasurement& rov, GnssMeasurement& ref) {
const double offset = 0.0; // shift a time offset for test// 用于测试的时间偏移
// Check if the queues are empty检查Rover和Reference队列是否为空
if (measurement_rov_.size() == 0) return false;
if (measurement_ref_.size() == 0) {
measurement_rov_.clear(); return false;
}
rov = measurement_rov_.front();//将Rover队列中的第一个测量值赋值给rov
measurement_rov_.pop_front();//将Rover队列中的第一个测量值弹出
// get the nearest timestamp
double min_dt = 1.0e6;
int index = -1;
for (int i = measurement_ref_.size() - 1; i >= 0; i--) {//遍历Reference队列，找到与Rover队列中最前面的测量最接近的时间戳的测量
double dt = fabs(rov.timestamp - measurement_ref_.at(i).timestamp - offset);
if (min_dt > dt) {
min_dt = dt; index = i;
}
}
CHECK(index != -1);//检查index是否为-1
ref = measurement_ref_.at(index);//将参考站队列指定时间戳的值赋值给ref
// pop all in front of this
double cut_timestamp = ref.timestamp;//
while (!checkEqual(cut_timestamp, measurement_ref_.front().timestamp)) {
measurement_ref_.pop_front();//从参考站队列中移除所有时间戳在ref之前的测量
}
// Check timestamps检查两个测量的时间戳是否在允许的最大时间差内
if (!checkEqual(rov.timestamp, ref.timestamp, max_age)) {
LOG(WARNING) << "Max age between two measurements exceeded! "
<< "age = " << fabs(rov.timestamp - ref.timestamp)
<< ", max_age = " << max_age;
return false;
}
return true;
}
// Solve current graph
bool RtkEstimator::estimate()
{ //将估计器状态初始化为"Converged"，表示估计器在当前迭代中已经收敛
status_ = EstimatorStatus::Converged;//初始化估计器状态
// Optimize with FDE使用FDE（Fault Detection and Exclusion）进行优化
size_t n_pseudorange = numPseudorangeError(curState());
size_t n_phaserange = numPhaserangeError(curState());
size_t n_doppler = numDopplerError(curState());
if (gnss_base_options_.use_outlier_rejection)//获取当前状态下估计器的伪距、相位距离和多普勒误差的数量
while (1)
{
optimize();// 进行优化
//进行优化并在每次迭代中拒绝异常值，直到没有异常值或达到最大迭代次数
// reject outlier 拒绝异常值
if (!rejectPseudorangeOutlier(curState(), curAmbiguityState(),
gnss_base_options_.reject_one_outlier_once) &&
!rejectDopplerOutlier(curState(),
gnss_base_options_.reject_one_outlier_once) &&
!rejectPhaserangeOutlier(curState(), curAmbiguityState(),
gnss_base_options_.reject_one_outlier_once)) break;
if (!gnss_base_options_.reject_one_outlier_once) break;
}
// Optimize without FDE如果未启用异常值拒绝，直接进行优化
else {
optimize();
}
// Check if we rejected too many residuals检查是否拒绝了过多的残差
double ratio_pseudorange = n_pseudorange == 0.0 ? 0.0 : 1.0 - //计算拒绝比例
getDivide(numPseudorangeError(curState()), n_pseudorange);
double ratio_phaserange = n_phaserange == 0.0 ? 0.0 : 1.0 -
getDivide(numPhaserangeError(curState()), n_phaserange);
double ratio_doppler = n_doppler == 0.0 ? 0.0 : 1.0 -
getDivide(numDopplerError(curState()), n_doppler);
const double thr = gnss_base_options_.diverge_max_reject_ratio;//定义拒绝比例的阈值thr，用于判断是否发生了过多的残差拒绝
if (isGnssGoodObservation() && //检查是否是良好的GNSS观测，并且至少有一种残差拒绝比例超过了设定的阈值
(ratio_pseudorange > thr || ratio_phaserange > thr || ratio_doppler > thr)) {
num_cotinuous_reject_gnss_++;//满足条件，则增加num_cotinuous_reject_gnss_计数器
}
else num_cotinuous_reject_gnss_ = 0;//否则，将计数器重置为0
if (num_cotinuous_reject_gnss_ > //检查是否连续拒绝了过多的残差
gnss_base_options_.diverge_min_num_continuous_reject) {
LOG(WARNING) << "Estimator diverge: Too many outliers rejected!";
status_ = EstimatorStatus::Diverged;//如果是，则将估计器状态设置为"Diverged"表示估计器发散
num_cotinuous_reject_gnss_ = 0;//将连续拒绝残差的计数器重置为0
}
// Ambiguity resolution
curState().status = GnssSolutionStatus::Float;//在进行模糊度解算之前，将当前状态的解定状态（status）设置为浮动（Float）
if (rtk_options_.use_ambiguity_resolution) {//是否启用了模糊度解算
// get covariance of ambiguities获取模糊度的协方差
Eigen::MatrixXd ambiguity_covariance;
std::vector<uint64_t> parameter_block_ids;//构建包含当前模糊度状态的参数块的ID向量
for (auto id : curAmbiguityState().ids) {
parameter_block_ids.push_back(id.asInteger());
}//使用图优化库（Ceres Solver）计算这些模糊度的协方差矩阵
graph_->computeCovariance(parameter_block_ids, ambiguity_covariance);
// solve
AmbiguityResolution::Result ret = ambiguity_resolution_->solveRtk(//调用模糊度解算器的 solveRtk 方法进行解算
curState().id, curAmbiguityState().ids, //传递当前状态的ID、当前模糊度状态的IDs、模糊度的协方差矩阵以及GNSS测量对。
ambiguity_covariance, gnss_measurement_pairs_.back());
if (ret == AmbiguityResolution::Result::NlFix) {//如果成功解算，则将状态设置为"Fixed"表示已成功固定模糊度
curState().status = GnssSolutionStatus::Fixed;
}
}
//检查在有良好的GNSS观测情况下，是否连续无法解定模糊度，从而判断是否需要清除当前的模糊度估计
// Check if we continuously cannot fix ambiguity, while we have good observations
if (rtk_options_.use_ambiguity_resolution) {//检查是否启用模糊度分辨
const double thr = gnss_base_options_.good_observation_max_reject_ratio;//定义拒绝比例阈值
if (isGnssGoodObservation() && ratio_pseudorange < thr && //检查是否为良好的GNSS观测
ratio_phaserange < thr && ratio_doppler < thr) {
//如果当前的GNSS解定状态不是已解定模糊度状态，增加连续无法解定模糊度的计数
if (curState().status != GnssSolutionStatus::Fixed) num_continuous_unfix_++;
else num_continuous_unfix_ = 0;//如果当前的GNSS解定状态是已解定模糊度状态，将计数器重置为0
}
else num_continuous_unfix_ = 0;
if (num_continuous_unfix_ > //如果连续无法解定模糊度的计数器超过了设定的最小连续无法解定次数，表示连续无法解定模糊度
gnss_base_options_.reset_ambiguity_min_num_continuous_unfix) {
LOG(INFO) << "Continuously unfix under good observations. Clear current ambiguities.";
resetAmbiguityEstimation();//清除当前的模糊度估计
num_continuous_unfix_ = 0;//将连续无法解定模糊度的计数器重置为0
}
}
// Log information
if (base_options_.verbose_output) {
int distance = static_cast<int>(round(
(curGnssRov().position - curGnssRef().position).norm() / 1.0e3));
LOG(INFO) << estimatorTypeToString(type_) << ": "
<< "Iterations: " << graph_->summary.iterations.size() << ", "
<< std::scientific << std::setprecision(3)
<< "Initial cost: " << graph_->summary.initial_cost << ", "
<< "Final cost: " << graph_->summary.final_cost
<< ", Sat number: " << std::setw(2) << num_satellites_
<< ", GDOP: " << std::setprecision(1) << std::fixed << gdop_
<< ", Dif distance: " << distance << " km"
<< ", Fix status: " << std::setw(1) << static_cast<int>(curState().status);
}
// Apply marginalization
marginalization();
// Shift memory for states and measurements将状态和测量值的内存进行迁移
shiftMemory();
return true;
}
参考一、5的meausrement_align_.add()
参考一、5的meausrement_align_.get()
// Add GNSS measurements and state
bool RtkImuCameraRrrEstimator::addGnssMeasurementAndState(
const GnssMeasurement& measurement_rov,
const GnssMeasurement& measurement_ref)
{
// Get prior states获取先验坐标转到 ECEF
Eigen::Vector3d position_prior = coordinate_->convert(
getPoseEstimate().getPosition(), GeoType::ENU, GeoType::ECEF);
// Set to local measurement handle设置本地当前量测值句柄
curGnssRov() = measurement_rov;//设置当前流动站量测值(此函数返回一个GnssMeasurement类型的引用)
curGnssRov().position = position_prior;//设置当前流动站位置
curGnssRef() = measurement_ref;//设置参考站量测值
//去除重复的相位观测值，并将它们整理成每个相位一个观测值
// Erase duplicated phases, arrange to one observation per phase
gnss_common::rearrangePhasesAndCodes(curGnssRov());//重新排列流动站相位和码观测值
gnss_common::rearrangePhasesAndCodes(curGnssRef());//重新排列基准站相位和码观测值
// Form double difference pair构造双差对
std::map<char, std::string> system_to_base_prn;//系统到基准站PRN映射
GnssMeasurementDDIndexPairs phase_index_pairs = gnss_common::formPhaserangeDDPair(//相位双差
curGnssRov(), curGnssRef(), system_to_base_prn, gnss_base_options_.common);
GnssMeasurementDDIndexPairs code_index_pairs = gnss_common::formPseudorangeDDPair(//伪距双差
curGnssRov(), curGnssRef(), system_to_base_prn, gnss_base_options_.common);
// Cycle-slip detection
if (!isFirstEpoch()) {//不是第一历元
cycleSlipDetectionSD(lastGnssRov(), lastGnssRef(),
curGnssRov(), curGnssRef(), gnss_base_options_.common);
}
// Add parameter blocks
double timestamp = curGnssRov().timestamp;//获取当前流动站量测值时间戳
// pose and speed and bias block
const int32_t bundle_id = curGnssRov().id;//获取当前流动站ID
BackendId pose_id = createGnssPoseId(bundle_id);//创建GNSS坐标系ID
size_t index = insertImuState(timestamp, pose_id);//在滑动窗口内部或两端插入IMU状态
states_[index].status = GnssSolutionStatus::Single;//状态设置为单点
latest_state_index_ = index;//更新 latest_state_index_ 以记录最新状态的索引
// GNSS extrinsics, it should be added at initialization step
CHECK(gnss_extrinsics_id_.valid());//检查GNSS外参是否有效。通常，这应该在初始化步骤中添加外参
// ambiguity blocks
addSdAmbiguityParameterBlocks(curGnssRov(), //添加与载波相位模糊度相关的参数块
curGnssRef(), phase_index_pairs, curGnssRov().id, curAmbiguityState());
// frequency block添加与频率相关的参数块
int num_valid_doppler_system = 0;
addFrequencyParameterBlocks(curGnssRov(), curGnssRov().id, num_valid_doppler_system);
// Add pseudorange residual blocks添加伪距残差块
int num_valid_satellite = 0;
addDdPseudorangeResidualBlocks(curGnssRov(),
curGnssRef(), code_index_pairs, states_[index], num_valid_satellite);
// We do not need to check if the number of satellites is sufficient in tightly fusion.
if (!checkSufficientSatellite(num_valid_satellite, 0)) {
// do nothing
}
num_satellites_ = num_valid_satellite;
// No satellite如果没有卫星可用
if (num_satellites_ == 0) {
// erase parameters in current state从当前状态中删除相关的参数块
eraseFrequencyParameterBlocks(states_[index]);
eraseImuState(states_[index]);
eraseAmbiguityParameterBlocks(curAmbiguityState());
return false;//并返回 false，表示无法继续估计
}
// Add phaserange residual blocks添加相位距离残差块和多普勒残差块，用于进一步优化GNSS解
addDdPhaserangeResidualBlocks(
curGnssRov(), curGnssRef(), phase_index_pairs, states_[index]);
// Add doppler residual blocks
addDopplerResidualBlocks(curGnssRov(), states_[index], num_valid_satellite,
false, getImuMeasurementNear(timestamp).angular_velocity);
// Add relative errors添加相对频率残差块和相对模糊度残差块，用于考虑相邻状态之间的相对误差
if (lastGnssState().valid()) { // maybe invalid here because of long term GNSS absent
// frequency
addRelativeFrequencyResidualBlock(lastGnssState(), states_[index]);
// ambiguity
addRelativeAmbiguityResidualBlock(
lastGnssRov(), curGnssRov(), lastAmbiguityState(), curAmbiguityState());
}
// ZUPT添加零速度更新（ZUPT）残差块，用于利用IMU测量的零速度信息
addZUPTResidualBlock(states_[index]);
// Car motion如果启用了车辆运动的模型，添加车辆运动的相关约束，包括航向角度和非完整约束
if (imu_base_options_.car_motion) {
// heading measurement constraint
addHMCResidualBlock(states_[index]);
// non-holonomic constraint
addNHCResidualBlock(states_[index]);
}
// Compute DOP计算位置精度因子
updateGdop(curGnssRov(), code_index_pairs);
return true;
}
在gnss_common.cpp/287行
该函数对每颗卫星处理GNSS测量数据，根据相位ID和默认码重新排列观测数据，并在必要时应用码偏。目标是确保每个相位只有一个观测值，并根据默认码进行排列。
//清除重复的相位，每个相位安排一个观测
// Erase duplicated phases, arrange to one observation per phase
void rearrangePhasesAndCodes(GnssMeasurement& measurement, bool accept_coarse)
{
CodeBiasPtr code_bias = measurement.code_bias;//获取code bias
for (auto& sat : measurement.satellites) {//遍历卫星
std::string prn = sat.first;//获取卫星的prn
char system = prn[0];//获取卫星系统
Satellite& satellite = sat.second;//卫星数据的主体
std::unordered_map<int, Observation>& observations = satellite.observations;//获取卫星的观测值
std::unordered_map<int, Observation> arranged_observations;//定义整理后的观测值的映射
for (auto it = observations.begin(); it != observations.end(); it++) {//遍历每个卫星的观测值
std::pair<int, Observation> arranged_observation;//存储重新排列后的观测数据，包含一个整数键和一个观测值对象
int code = it->first;//获取观测值对应的code
Observation observation = it->second;//获取观测主体
int phase_id = getPhaseID(system, code);//获取观测相位ID
int default_code = 0;
#define MAP(S, P, C) \
if (system == S && phase_id == P) { default_code = C; }//根据系统，相位ID，将默认的code设为对应的'C'
PHASE_CHANNEL_TO_DEFAULT_CODE;
#undef MAP //取消宏定义
bool can_add = true;
// do not need to arrange当前观测值匹配到默认code，直接添加
if (default_code == code) arranged_observation = *it;//直接将观测值放入整理后的观测值映射中
// arrange to default code
else {//排列为默认码
double bias = code_bias->getCodeBias(prn, code, accept_coarse);//获取码偏
double default_bias =
code_bias->getCodeBias(prn, default_code, accept_coarse);
// do not have code bias
if (bias == 0.0 || default_code == 0.0) {
// consider DCBs in the same frequency are zeros考虑在同一频率的DCB是零
if (accept_coarse) {//如果允许使用粗频
arranged_observation = std::make_pair(default_code, observation);//将观测值放入整理后的观测值映射中
}
// cannot arrange
else {
// pass
can_add = false;
}
}
// use code bias to arrange
else {
observation.pseudorange += bias - default_bias;//将观测值中的伪距加上偏移
arranged_observation = std::make_pair(default_code, observation);//将观测值放入整理后的观测值映射中
}
}
// add to observations
if (can_add) {
if (arranged_observations.find(default_code) == arranged_observations.end()) {//如果整理后的观测值映射中不存在该code
arranged_observations.insert(arranged_observation);//将整理后的卫星观测值放入整理后的卫星观测值映射中
}
}
}
satellite.observations = arranged_observations;
}
}
该函数formPhaserangeDDPair() 存在重载，此处在gnss_common.cpp/626行调用
// Form double difference phaserange pair
GnssMeasurementDDIndexPairs formPhaserangeDDPair(
const GnssMeasurement& measurement_rov,
const GnssMeasurement& measurement_ref,
std::map<char, std::string>& system_to_base_prn,
const GnssCommonOptions& options)
{
// Form SD pair
GnssMeasurementSDIndexPairs sd_pairs = formPhaserangeSDPair(
measurement_rov, measurement_ref, options);//生成单差对，输出索引
// Prepare data定义初始化各种映射对
std::map<char, int> system_to_num_phases;//key键为系统，value值为该系统卫星最大频率数(相位出现的最大次数)
std::multimap<char, double> system_to_phases;
std::map<std::string, int> prn_to_number_phases; //key键为prn，value值为prn计数
std::multimap<std::string, double> prn_to_phases;//key为prn，value为相位ID
std::multimap<std::string, int> prn_to_indexes;//key为prn，value为索引
for (size_t i = 0; i < sd_pairs.size(); i++) {//循环遍历生成的单差对(站间单差)
std::string prn = measurement_rov.getSat(sd_pairs[i].rov).prn;//获取观测卫星的PRN
auto it = prn_to_number_phases.find(prn);//通过prn(key键)找到相应的映射对
if (it == prn_to_number_phases.end()) {//如果迭代器找不到，生成新的prn对，值设为1
prn_to_number_phases.insert(std::make_pair(prn, 1));
}
else it->second++;//否则值加1
prn_to_indexes.insert(std::make_pair(prn, i));//key为prn，value为索引
int code = sd_pairs[i].rov.code_type;//获取码类型
double wavelength = measurement_rov.getObs(sd_pairs[i].rov).wavelength;//获取波长
int phase_id = getPhaseID(prn[0], code);//获取卫星系统相位ID
prn_to_phases.insert(std::make_pair(prn, phase_id));//插入对，key为prn，value为相位ID
}
后续需要对map,multimap容器做充分的了解才能很好地理解以下代码内容，从网上找来了一些知识：
参考链接：
C++STL | map/multimap容器和对组pair-CSDN博客
std::map 成员函数：lower_bound 与 upper_bound_map lower_bound函数-CSDN博客
lower_bound返回第一个大于等于val值的位置
upper_bound返回第一个大于val值的位置
for (size_t i = 0; i < getGnssSystemList().size(); i++) {//循环遍历GNSS系统列表
char system = getGnssSystemList()[i];//获取当前系统
system_to_num_phases.insert(std::make_pair(system, 0));//初始化映射对，value值为0
for (auto it : prn_to_number_phases) {//循环遍历prn对，value为该prn出现的次数
if (it.first[0] != system) continue;//如果PRN的首字母与当前系统不匹配，继续循环
if (system_to_num_phases.at(system) < it.second) {//当系统计数小于prn出现次数时，更新
system_to_num_phases.at(system) = it.second;//记录该系统的卫星最大频率数(相位出现的最大次数)
}
}
for (auto it : prn_to_phases) {//循环遍历prn对，value为相位ID
if (it.first[0] != system) continue;//PRN的首字母与当前系统不匹配,跳过
if (system_to_num_phases.find(system) == system_to_num_phases.end()) {//该系统映射对不存在时
system_to_num_phases.insert(std::make_pair(system, it.second));//插入相位ID对?
}
bool found = false;//设置未找到的标识符
for (auto it_wave = system_to_num_phases.lower_bound(system); //获取插入的映射对
it_wave != system_to_num_phases.upper_bound(system); it_wave++) {
if (it_wave->second == it.second) {//检查是否插入成功
found = true; break;//如果插入成功，将标识符设为true
}
}//如果未插入成功，再次插入
if (!found) system_to_num_phases.insert(std::make_pair(system, it.second));
}
}
// Find base satellites for each system and phases为每个卫星系统选择一个基准卫星
for (size_t i = 0; i < getGnssSystemList().size(); i++) {//循环遍历GNSS系统列表
char system = getGnssSystemList()[i];//获取当前系统字符
// check if we already selected a base检查是否已选择基准卫星
if (system_to_base_prn.find(system) != system_to_base_prn.end() && //该系统存在基准卫星
!system_to_base_prn.at(system).empty()) continue;//并且基准卫星不为空时，跳过
double max_elevation = 0.0;//初始化最大仰角
for (size_t j = 0; j < sd_pairs.size(); j++) {//循环遍历单差对
if (sd_pairs[j].rov.prn[0] != system) continue;//如果单差对中卫星的PRN的首字母与当前系统不匹配，则跳过当前循环
// we only select satellites with max phase number选择相位数最大的卫星
if (prn_to_number_phases.at(sd_pairs[j].rov.prn) != //该卫星的相位数
system_to_num_phases.at(system)) continue;//该系统最大相位数
double elevation = satelliteElevation(//计算当前卫星相对于参考站的仰角
measurement_ref.getSat(sd_pairs[j].ref).sat_position,
measurement_ref.position);
if (max_elevation < elevation) {//如果当前卫星的仰角大于最大仰角
system_to_base_prn[system] = sd_pairs[j].rov.prn;//更新基准卫星为当前卫星
max_elevation = elevation;//更新最大仰角
}
}
}
// Form DD pair生成双差对
GnssMeasurementDDIndexPairs dd_pairs;//初始化双差对对象
for (size_t i = 0; i < sd_pairs.size(); i++) {//循环遍历单差对
char system = sd_pairs[i].rov.prn[0];//获取系统标识符
std::string prn = sd_pairs[i].rov.prn;//获取prn
std::string prn_base = system_to_base_prn.at(system);//获取当前系统的基准卫星PRN
if (prn == prn_base) continue;//如果该卫星是基准卫星，跳过
for (auto it = prn_to_indexes.lower_bound(prn_base); //获取基准卫星索引
it != prn_to_indexes.upper_bound(prn_base); it++) {
GnssMeasurementSDIndexPair& sd_pair_base = sd_pairs[it->second];//获取基准卫星的单差对
int phase_id_base = getPhaseID(system, sd_pair_base.rov.code_type);//获取基准卫星相位的ID
int phase_id = getPhaseID(system, sd_pairs[i].rov.code_type);//获取当前卫星相位的ID
if (phase_id_base == phase_id) {//基准卫星的相位ID等于当前卫星的相位ID(同频)
dd_pairs.push_back(GnssMeasurementDDIndexPair(//将当前卫星和基准卫星的单差对组合成双差对，并将其加入到双差对中
sd_pairs[i].rov, sd_pairs[i].ref, sd_pair_base.rov, sd_pair_base.ref));
break;
}
}
}
函数在在gnss_common.cpp/466行调用
// Form single difference phaserange pair
GnssMeasurementSDIndexPairs formPhaserangeSDPair(
const GnssMeasurement& measurement_rov,
const GnssMeasurement& measurement_ref,
const GnssCommonOptions& options)
{
GnssMeasurementSDIndexPairs index_pairs;//初始化单差相位对索引对象
// Find valid observations in measurement_rov查找流动站观测值中的有效观测值
std::vector<GnssMeasurementIndex> indexes_1;//初始化观测值索引列表
for (auto& sat : measurement_rov.satellites) //遍历流动站所有卫星
{
auto& satellite = sat.second;
char system = satellite.getSystem();//获取卫星的标识符
if (!gnss_common::useSystem(options, system)) continue;//如果卫星系统不可使用，则跳过
for (auto obs : satellite.observations) {//遍历该卫星的观测数据
GnssMeasurementIndex index_rov(satellite.prn, obs.first);//表示卫星prn和观测类型的索引对象
if (checkObservationValid(measurement_rov, index_rov,//检查该观测值是否有效
ObservationType::Phaserange, options) &&
checkObservationValid(measurement_rov, index_rov,
ObservationType::Pseudorange, options)) {
indexes_1.push_back(index_rov);//将索引加入索引列表
}
}
}
// Find valid matches in measurement_ref在参考站观测值中找到有效的匹配
for (auto index : indexes_1) //遍历已选取的流动站有效观测的索引
{
auto& satellite = measurement_rov.satellites.at(index.prn);//获取该流动站的卫星
auto& observation = satellite.observations.at(index.code_type);//获取该该卫星的观测值
int phase_id = gnss_common::getPhaseID(satellite.getSystem(), index.code_type);//获取该卫星的相位ID(区分频率)
for (auto& sat : measurement_ref.satellites) {//遍历参考站所有卫星
auto& satellite_2 = sat.second;//获取该卫星的主要信息
if (satellite_2.prn != satellite.prn) continue;//当不是共视卫星时，跳过
//找到共视卫星
for (auto& obs_2 : satellite_2.observations) {//遍历参考站共视卫星的观测值
if (index.code_type != obs_2.first) continue;//当参考站与流动站的观测值类型不同时，跳过
auto& observation_2 = obs_2.second;//获取该参考站共视卫星的观测值
if (!checkObservationValid(measurement_ref, //检查该参考站共视卫星的观测值是否有效
GnssMeasurementIndex(satellite.prn, index.code_type),
ObservationType::Pseudorange)) continue;
if (!checkObservationValid(measurement_ref,
GnssMeasurementIndex(satellite.prn, index.code_type),
ObservationType::Phaserange)) continue;
index_pairs.push_back(GnssMeasurementSDIndexPair(//生成单差相位对，并添加的索引对中
index, GnssMeasurementIndex(satellite.prn, index.code_type)));
}
}
}
该函数formPseudorangeDDPair() 存在重载，此处在gnss_common.cpp/522行调用。伪距双差与相位双差的逻辑大致一样
周跳探测函数，没找到在何处，理论上来说应该在gnss_estimator_bash类中。
在imu_estimator_base.cpp/506行
在estimator_types.h/82行可以找到BackendId类的具体内容。
// Inserts a state inside or at the ends of state window
size_t ImuEstimatorBase::insertImuState(
const double timestamp, const BackendId& backend_id,//当前观测值时间戳和后端id
const Transformation& T_WS_prior,
const SpeedAndBias& speed_and_bias_prior,
const bool use_prior)
{
imu_state_mutex_.lock();
// Get the latest state获取最新的状态
bool has_invalid_state = false;//标记最新状态是否无效
double latest_timestamp = 0.0;
int valid_state_index = states_.size();//记录有效状态的索引
for (auto it = states_.rbegin(); it != states_.rend(); it++) {//逆序遍历现有状态
valid_state_index--;
if (it->valid()) {//如果状态有效
latest_timestamp = it->timestamp; break;//获取最新的时间戳，结束循环
}
else has_invalid_state = true;//如果状态无效，则标记为无效状态
}
// Free the pre-allocated memory
if (has_invalid_state) states_.pop_back();//如果最新状态无效，则删除最新的状态
// Check if it overlaps with existing state检查观测时间戳是否与状态时间戳重叠
int overlap_index = -1;
for (size_t i = 0; i < states_.size(); i++) {//从头遍历状态列表
if (checkEqual(states_[i].timestamp, timestamp)) {//如果状态时间戳与观测时间戳相同，则重叠
overlap_index = i; break;//记录状态重叠索引，结束循环
}
}
// Overlap with existing state存在重叠状态时，再重叠状态后插入新状态
if (overlap_index >= 0) //重叠状态索引存在时
{
auto it_lhs = states_.begin();
std::advance(it_lhs, overlap_index + 1);//通过状态索引获取重叠状态后一个状态迭代器
State state;//创建新状态
state.timestamp = timestamp;//新状态的时间戳=观测时间戳
state.id = backend_id;//新状态的id=后端id
state.id_in_graph = states_[overlap_index].id_in_graph;//新状态图优化的id=重叠状态图优化的id
state.imu_residual_to_lhs = states_[overlap_index].imu_residual_to_lhs;
auto it_cur = states_.insert(it_lhs, state);//在找到的位置插入新状态，并返回一个迭代器指向新插入的状态
if (State::overlaps.count(state.id_in_graph) == 0) {//如果如果之前未记录过这个图优化ID
State::overlaps.insert(std::make_pair(state.id_in_graph, states_[overlap_index]));//插入重叠状态的图优化ID
}//否则插入新状态的图优化ID
State::overlaps.insert(std::make_pair(state.id_in_graph, (*it_cur)));
imu_state_mutex_.unlock();//释放互斥锁
return overlap_index + 1;//返回新状态索引
}
// At the front of the window在窗口前插入状态
else if (latest_timestamp == 0.0 || //如果最新有效状态的时间戳为0
checkLessEqual(timestamp, oldestState().timestamp))//或者当前观测值时间戳小于最早状态的时间戳
{
CHECK(use_prior) << "Cannot add new IMU state at the front of the state "
<< "window without motion prior!";//必须使用先验信息
// add parameter block at front向状态窗口的最前面添加与位置、姿态、速度和相关偏差的参数块
addPoseParameterBlock(backend_id, T_WS_prior);
addImuSpeedAndBiasParameterBlock(backend_id, speed_and_bias_prior);
State state;//创建新状态并插入状态窗口前面
state.timestamp = timestamp;
state.id = backend_id;
state.id_in_graph = backend_id;
states_.push_front(state);
// connect current state to the next连接当前状态与下一个状态
if (states_.size() > 1) {//如果状态窗口中有多于一个状态
addImuResidualBlock(states_[0], states_[1]);//连接当前状态与下一个状态
}
imu_state_mutex_.unlock();
return 0;
}
// At the end of the window在窗口后端插入状态
else if (checkLessEqual(latest_timestamp, timestamp)) //如果最新状态的时间戳小于当前观测值时间戳
{
// add parameter block at back在末尾添加参数块
const State& last_state = states_[valid_state_index];//获取最新的有效状态
Transformation T_WS = T_WS_prior;
SpeedAndBias speed_and_bias = speed_and_bias_prior;
if (!use_prior) {//不使用先验信息时
getMotionFromEstimateAndImuIntegration(//从估计值和IMU积分中获取运动信息
last_state, timestamp, T_WS, speed_and_bias);
}
addPoseParameterBlock(backend_id, T_WS);//添加位置、姿态参数块
addImuSpeedAndBiasParameterBlock(backend_id, speed_and_bias);//添加速度和偏差参数块
State state;//创建新状态并插入窗口末尾
state.timestamp = timestamp;
state.id = backend_id;
state.id_in_graph = backend_id;
states_.push_back(state);
// connect the last state to current连接最新的状态和当前观测状态
addImuResidualBlock(lastState(), curState());
imu_state_mutex_.unlock();
return states_.size() - 1;
}
// Inside the window, we break the IMU connections and reform them
else
{
// find two states around the timestamp查找两个相邻时间戳的状态，观测时间戳在中间
size_t index_lhs, index_rhs;
for (size_t i = states_.size() - 1; i >= 0; i--) {//从后往前遍历状态队列
State& state = states_[i];
if (state.valid() && state.timestamp > timestamp) {//如果状态有效且时间戳大于观测值时间戳
index_rhs = i;//记录后端状态索引
}
if (state.valid() && state.timestamp <= timestamp) {//如果状态有效且时间戳小于等于观测值时间戳
index_lhs = i;//记录前端状态索引
break;//结束循环
}
}//此时观测时间戳在中间
// add parameter blocks添加参数块
Transformation T_WS = T_WS_prior;
SpeedAndBias speed_and_bias = speed_and_bias_prior;
if (!use_prior) {//不使用先验信息时
getMotionFromEstimateAndImuIntegration(//通过前端状态的估计值和IMU积分获取运动信息
states_[index_lhs], timestamp, T_WS, speed_and_bias);
}
addPoseParameterBlock(backend_id, T_WS);//添加位置、姿态参数块
addImuSpeedAndBiasParameterBlock(backend_id, speed_and_bias);//添加速度和偏差参数块
auto it_lhs = states_.begin(); std::advance(it_lhs, index_lhs + 1);//获取左侧状态之后的迭代器
State state;
state.timestamp = timestamp;//设置时间戳
state.id = backend_id;//设置后端ID
state.id_in_graph = backend_id;//设置图优化ID
auto it_cur = states_.insert(it_lhs, state);//在左侧状态之后插入新状态，并获取指向新状态的迭代器
State& cur_state = *it_cur;//当前状态的迭代器
auto it_rhs_new = it_cur; it_rhs_new++;//获取下一状态的迭代器
auto it_lhs_new = it_cur; it_lhs_new--;//获取上一状态的迭代器
const State& state_lhs = *it_lhs_new;//更新上一状态的迭代器
State& state_rhs = *it_rhs_new;//更新下一状态的迭代器
// erase old IMU connection删除旧的IMU连接关系
eraseImuResidualBlock(state_lhs, state_rhs);
// add LHS IMU connection添加新状态与上一时刻状态的残差块
addImuResidualBlock(state_lhs, cur_state);
// add RHS IMU connection添加新状态与下一时刻状态的残差块
addImuResidualBlock(cur_state, state_rhs);
imu_state_mutex_.unlock();
return index_lhs + 1;
}
imu_state_mutex_.unlock();
// Visual initialization
bool RtkImuCameraRrrEstimator::visualInitialization(const FrameBundlePtr& frame_bundle)
{
// store poses
do_not_remove_imu_measurements_ = true;//在视觉初始化过程中不要删除IMU测量
Solution solution;
solution.timestamp = getTimestamp();//设置时间戳
solution.pose = getPoseEstimate();//获取当前位姿
solution.speed_and_bias = getSpeedAndBiasEstimate();//获取速度和偏差
init_solution_store_.push_back(solution);
// store keyframes
if (frame_bundle->isKeyframe()) {//如果当前帧是关键帧
init_keyframes_.push_back(frame_bundle);//将帧添加到关键帧列表中
while (init_keyframes_.size() > 2) init_keyframes_.pop_front();//如果关键帧列表大于2，则删除第一个元素
}
if (init_keyframes_.size() < 2) return false;//关键帧数量不足两个
// check if GNSS/IMU estimator fully converged 检查GNSS/IMU估计是否完全收敛
double std_yaw = sqrt(computeAndGetCovariance(lastState())(5, 5));//计算最后一个状态偏航角的协方差
if (std_yaw > rrr_options_.min_yaw_std_init_visual * D2R) return false;//如果偏航方向的标准差大于设定的最小偏航标准差的阈值
// set poses
std::vector<SpeedAndBias> speed_and_biases;//存储速度和偏差的向量
for (auto& frame_bundle : init_keyframes_) {//遍历每个关键帧
bool found = false;//标记是否找到了与关键帧对应的GNSS/IMU解
double timestamp = frame_bundle->getMinTimestampSeconds();//获取关键帧的最小时间戳
for (size_t i = 1; i < init_solution_store_.size(); i++) {//遍历所有存储的GNSS/IMU解
double dt1 = init_solution_store_[i].timestamp - timestamp;//计算当前解与关键帧时间戳的时间差
double dt2 = init_solution_store_[i - 1].timestamp - timestamp;//计算前一个解与关键帧时间戳的时间差
if ((dt1 >= 0 && dt2 <= 0) || //关键帧是否在两个解之间或者
(i == init_solution_store_.size() - 1) && dt1 < 0 && fabs(dt1) < 2.0) {//在当前解之后不超过2秒
size_t idx = (dt1 >= 0 && dt2 <= 0) ? (i - 1) : i;//确定要使用的GNSS/IMU解的索引
Transformation T_WS = init_solution_store_[idx].pose;//获取解的位姿
SpeedAndBias speed_and_bias = init_solution_store_[idx].speed_and_bias;//获取解的速度和偏差
imuIntegration(init_solution_store_[idx].timestamp, //将IMU状态递推到关键帧时间戳上
timestamp, T_WS, speed_and_bias);
speed_and_biases.push_back(speed_and_bias);//添加IMU递推的速度和偏差
frame_bundle->set_T_W_B(T_WS);//设置关键帧的位姿
found = true;//设置找到与关键帧对应的状态解
break;
}
}
CHECK(found);
}
// initialize landmarks
feature_handler_->initializeLandmarks(init_keyframes_.back()->at(0));//初始化最新的关键帧的路标点
feature_handler_->setGlobalScaleInitialized();//全局尺度初始化
// add two keyframes将图像测量和状态添加到图优化中
CHECK(addImageMeasurementAndState(init_keyframes_.front(), speed_and_biases.front()));
CHECK(addImageMeasurementAndState(init_keyframes_.back(), speed_and_biases.back()));
// set flag
visual_initialized_ = true;
do_not_remove_imu_measurements_ = false;
can_compute_covariance_ = true;
return true;
}
在feature_handler.cpp/493
// Initialize landmarks
void FeatureHandler::initializeLandmarks(const FramePtr& keyframe)
{
const FramePtr& frame = keyframe;
if (!frame->isKeyframe()) return;//检查是否是关键帧
mutex_.lock();//确保在多线程下安全访问数据
// Initialize landmarks
for (size_t i = 0; i < frame->numFeatures(); i++) {//遍历关键帧的所有特征点
auto& landmark = frame->landmark_vec_[i];//获取特征点对应的路标点
CHECK(landmark != nullptr);
// already initialized
if (isSeed(frame->type_vec_[i])) continue;//如果路标点初始化过，则跳过
// rejected by bundle adjuster 跳过被标记为outlier的特征点
if (frame->type_vec_[i] == FeatureType::kOutlier) continue;
// get the last keyframe
FramePtr ref_frame = nullptr;
size_t index_ref = 0;
for (auto obs = landmark->obs_.rbegin(); obs != landmark->obs_.rend(); obs++) {//反向遍历当前路标点的观测值
if (FramePtr f = obs->frame.lock()) {//获取观测值的帧
if (f->isKeyframe() && f->id() < frame->id() && f->has_transform_) {//如果是关键帧且比当前帧更早
ref_frame = f; //记录该参考关键帧
index_ref = obs->keypoint_index_;//记录参考关键帧在该路标点观测值中的索引
}
}
}
if (ref_frame == nullptr) continue;//如果没有参考帧，则跳过
if (ref_frame->id() == frame->id()) continue;//如果参考帧是当前帧，则跳过
// check disparity
double disparity = (ref_frame->px_vec_.col(index_ref) -
frame->px_vec_.col(i)).norm();//计算当前地图点在参考关键帧和当前关键帧之间的视差
if (disparity < options_.min_disparity_init_landmark) continue;//如果视差小于设定的最小视差阈值，则跳过
//计算当前关键帧到参考关键帧的变换矩阵
Transformation T_cur_ref = frame->T_f_w_ * ref_frame->T_f_w_.inverse();
// avoid pure rotation
if (T_cur_ref.getPosition().norm() < //相对姿态的平移部分的大小如果小于设定的最小平移阈值，则跳过当前路标点的初始化
options_.min_translation_init_landmark) continue;
BearingVector f_ref = ref_frame->f_vec_.col(index_ref);//获取参考帧上特征点的方向向量
BearingVector f_cur = frame->f_vec_.col(i);//获取当前帧上特征点的方向向量
double depth;
if (!(depthFromTriangulation(T_cur_ref, f_ref, f_cur, &depth) //三角法计算深度
== FeatureMatcher::MatchResult::kSuccess)) continue;//如果计算深度失败，则跳过
if (depth < 0.1) continue;//如果深度小于0.1，则跳过
// Note that the follow equation should not be T * f_ref * depth.
// Because the operater "*" is applied in homogeneous coordinate
landmark->pos_ = ref_frame->T_world_cam() * (f_ref / f_ref(2) * depth);//计算当前路标点的三维坐标
//归一化相机坐标乘以相机到世界坐标的变换矩阵
// Change feature type
for (auto obs : landmark->obs_) {//遍历当前路标点的观测值
if (FramePtr f = obs.frame.lock()) {//获取观测值的帧
changeFeatureTypeToSeed(f->type_vec_[obs.keypoint_index_]);//将观测值对应的特征点类型设置为已初始化
}
}
}
mutex_.unlock();
}
// Add image measurements and state
bool RtkImuCameraRrrEstimator::addImageMeasurementAndState(
const FrameBundlePtr& frame_bundle, const SpeedAndBias& speed_and_bias)
{
// If initialized, we are supposed not at the first epoch
CHECK(!isFirstEpoch());//检查是否已经初始化。如果已经初始化，那么应该不处于第一个时刻
// Check frequency
if (!frame_bundle->isKeyframe() && frame_bundles_.size() > 1) {//如果不是关键帧且已经有至少一个图像帧被添加
const double t_last = lastFrameBundle()->getMinTimestampSeconds();//获取上一帧的时间戳
const double t_cur = frame_bundle->getMinTimestampSeconds();//获取当前帧的时间戳
if (t_cur - t_last < 1.0 / visual_base_options_.max_frequency) return false;//如果两帧时间间隔小于最大频率对应的时间间隔，报错
}//当时间间隔大于1秒时，不考虑最大频率
// Set to local measurement handle设置到本地测量句柄
curFrameBundle() = frame_bundle;
// Add parameter blocks
double timestamp = curFrame()->getTimestampSec();//获取当前帧的时间戳
// pose and speed and bias block
const int32_t bundle_id = curFrame()->bundleId();//获取当前帧的ID
BackendId pose_id = createNFrameId(bundle_id);//创建标识位姿的ID
size_t index;
if (speed_and_bias != SpeedAndBias::Zero()) {//如果速度和偏差不为零，则添加位姿，速度和偏移参数块
index = insertImuState(timestamp, pose_id,
curFrame()->T_world_imu(), speed_and_bias, true);
}
else {//当速度和偏差为零时，直接添加位姿参数块
index = insertImuState(timestamp, pose_id);
}
states_[index].status = latestGnssState().status;//将新插入的IMU状态的状态标记为最新的GNSS状态的标记
states_[index].is_keyframe = curFrame()->isKeyframe();//标记是否为关键帧
latest_state_index_ = index;//更新插入的IMU状态的索引
// camera extrinsics 相机外参
if (!camera_extrinsics_id_.valid()) {//如果相机外参ID不存在
camera_extrinsics_id_ =
addCameraExtrinsicsParameterBlock(bundle_id, curFrame()->T_imu_cam());//添加相机外参参数块
addCameraExtrinsicsResidualBlock(camera_extrinsics_id_, curFrame()->T_imu_cam(), //添加相机外参残差块
visual_base_options_.camera_extrinsics_initial_std.head<3>(),
visual_base_options_.camera_extrinsics_initial_std.tail<3>() * D2R);
}
// Initialize landmarks
if (visual_initialized_ && curFrame()->isKeyframe()) {//如果视觉初始化已经完成，并且当前帧是关键帧
curFrame()->set_T_w_imu(getPoseEstimate(states_[index]));//设置当前帧的位姿
feature_handler_->initializeLandmarks(curFrame());//初始化路标点
}
// Add Landmark parameters and minimal resiudals at keyframe
if (curFrame()->isKeyframe()) {
addLandmarkParameterBlocksWithResiduals(curFrame());//添加路标点参数块及残差
}
// Add landmark observations
addReprojectionErrorResidualBlocks(states_[index], curFrame());//添加重投影误差残差块
return true;
}
// Add landmark blocks to graph
void VisualEstimatorBase::addLandmarkParameterBlocksWithResiduals(const FramePtr& frame)
{
// only add new landmarks at keyframe
if (!frame->isKeyframe()) return;
double timestamp = frame->getTimestampSec();//获取当前帧的时间戳
for (size_t kp_idx = 0; kp_idx < frame->numFeatures(); ++kp_idx)//遍历当前帧的每一个特征点
{
const PointPtr& point = frame->landmark_vec_[kp_idx];//获取当前特征点对应的路标点
const FeatureType& type = frame->type_vec_[kp_idx];//获取当前特征点的类型
// check if feature is associated to landmark
if (point == nullptr || !isSeed(frame->type_vec_[kp_idx])) {//路标点为空或没有被初始化过
continue;
}
// add new landmarks and corresponding observations at keyframe
if (!isLandmarkInEstimator(createLandmarkId(point->id()))) {//路标点没有被添加到图优化中
// check if the first frame is current keyframe
if (point->obs_.front().frame_id == frame->id()) {//当前帧是路标点第一个观测帧，跳过
continue;
}
CHECK(!std::isnan(point->pos_[0])) << "Point is nan!";
// add the landmark
BackendId landmark_backend_id = createLandmarkId(point->id());//创建路标点的后端ID
std::shared_ptr<HomogeneousPointParameterBlock>
point_parameter_block =//创建路标点参数块
std::make_shared<HomogeneousPointParameterBlock>(
point->pos(), landmark_backend_id.asInteger());//添加位置参数，id
CHECK(graph_->addParameterBlock(
point_parameter_block, Graph::HomogeneousPoint));
// add landmark to map
auto it_landmark = landmarks_map_.emplace_hint(//添加路标点到图中
landmarks_map_.end(),
landmark_backend_id, MapPoint(point));//使用路标点后端id作为键
point->in_ba_graph_ = true;//标记路标点已经添加到图中
// add two observations (current frame and the last keyframe)
size_t added_cnt = 0;//初始化记录添加的观测个数
for (auto obs = point->obs_.rbegin(); obs != point->obs_.rend(); obs++) {//使用反向迭代器遍历当前路标点的所有观测值
// check if frames ahead
if (obs->frame_id > frame->id()) continue;//如果观测值对应帧的id大于当前帧，跳过
if (FramePtr f = obs->frame.lock()) {//获取观测值对应的帧
if (f->isKeyframe()) {//如果观测值对应的帧是关键帧
State state;
for (auto s = states_.rbegin(); s != states_.rend(); s++) {//使用反向迭代器遍历所有状态
if (!s->valid()) continue;//如果状态无效，跳过
if (s->id != createNFrameId(f->bundleId())) continue;//如果状态对应的帧id不等于观测值对应的帧id，跳过
state = *s; break;
}
if (!state.valid()) continue;//如果状态无效，跳过
addReprojectionErrorResidualBlock(state, f, obs->keypoint_index_);//添加重投影误差残差块
added_cnt++; //
}
}
if (added_cnt >= 2) break;//如果成功添加的观测数量达到2个，则跳出循环
}
// invalid landmark
if (added_cnt < 2) {//如果成功添加的观测数量不足2个，说明该路标点无效
graph_->removeParameterBlock(landmark_backend_id.asInteger());//从图优化的参数块中移除该路标点的参数块
point->in_ba_graph_ = false;//标记路标点已经从图中移除
landmarks_map_.erase(it_landmark);//从图中移除该路标点
}
}
}
// Store keyframes
active_keyframes_.push_back(frame); //将当前帧添加到活动关键帧中
}
// Add landmark reprojection error block
ceres::ResidualBlockId VisualEstimatorBase::addReprojectionErrorResidualBlock(
const State& state, const FramePtr& frame, const size_t keypoint_idx)
{
const BackendId pose_id = createNFrameId(frame->bundleId());//创建帧的位姿后端ID
CHECK(state.id == pose_id);
CHECK_GE(frame->level_vec_(keypoint_idx), 0);
const int cam_idx = frame->getNFrameIndex();//获取帧的相机索引
// get Landmark ID.
const BackendId landmark_backend_id = createLandmarkId(//创建路标点的后端ID
frame->track_id_vec_[keypoint_idx]);
if (!isLandmarkInEstimator(landmark_backend_id)) {//如果路标点不在图优化中，报错
LOG(WARNING) << "Landmark " << landmark_backend_id << " not added!";
return nullptr;
}
KeypointIdentifier kid(frame, keypoint_idx);//创建关键点标识符
// check for double observations 确保不添加重复的路标点观测值
const auto& obs = landmarks_map_.at(landmark_backend_id).observations.find(kid);//查找路标点观测值
if (obs != landmarks_map_.at(landmark_backend_id).observations.end()) {//如果路标点观测值存在
return ceres::ResidualBlockId(obs->second);//返回观测值对应的残差块ID
}
Eigen::Matrix2d information = Eigen::Matrix2d::Identity();//初始化信息矩阵
if (landmarks_map_.at(landmark_backend_id).num_observations_historical >
visual_base_options_.min_observation_stable) {//如果路标点历史观测次数大于视觉基础选项中定义的最小稳定观测次数
information /= square(visual_base_options_.stable_feature_error_std *
static_cast<double>(1 << frame->level_vec_(keypoint_idx)));//使用稳定特征误差标准差和关键点层级来调整信息矩阵
}
else {//否则，使用普通特征误差标准差和关键点层级来调整信息矩阵
information /= square(visual_base_options_.feature_error_std *
static_cast<double>(1 << frame->level_vec_(keypoint_idx)));
}
// create error term
std::shared_ptr<ReprojectionError> reprojection_error=//创建重投影误差
std::make_shared<ReprojectionError>(
frame->cam(),//相机参数
frame->px_vec_.col(keypoint_idx), information);//关键点像素坐标、信息矩阵
ceres::ResidualBlockId ret_val = graph_->addResidualBlock(//将重投影误差项添加到图优化中
reprojection_error,//重投影误差项
cauchy_loss_function_ ? cauchy_loss_function_.get() : nullptr,//损失函数
graph_->parameterBlockPtr(state.id_in_graph.asInteger()),//帧位姿参数块
graph_->parameterBlockPtr(landmark_backend_id.asInteger()),//路标点参数块
graph_->parameterBlockPtr(camera_extrinsics_id_.asInteger()));//相机外参参数块
// remember 记录该观测信息，将新的观测插入到路标点点的 observations 映射中
landmarks_map_.at(landmark_backend_id).observations.insert(
std::pair<KeypointIdentifier, uint64_t>(
kid, reinterpret_cast<uint64_t>(ret_val)));
landmarks_map_.at(landmark_backend_id).num_observations_historical++;//更新路标点历史观测次数
return ret_val;
}
在336行
// Solve current graph
bool RtkImuCameraRrrEstimator::estimate()
{
status_ = EstimatorStatus::Converged;
// Optimize图优化
optimize();
// Get current sensor type
IdType new_state_type = states_[latest_state_index_].id.type();//获取当前状态类型
// GNSS processes
double gdop = 0.0;
if (new_state_type == IdType::gPose)//如果最新的状态是GNSS测量结果
{
// Reject GNSS outliers拒绝GNSS测量的离群值
size_t n_pseudorange = numPseudorangeError(states_[latest_state_index_]);//获取最新状态下的伪距测量数量
size_t n_phaserange = numPhaserangeError(states_[latest_state_index_]);//获取最新状态下的相位测量数量
size_t n_doppler = numDopplerError(states_[latest_state_index_]);//获取最新状态下的Doppler测量数量
if (gnss_base_options_.use_outlier_rejection) {//检查配置选项，确定是否启用了离群值拒绝
rejectPseudorangeOutlier(states_[latest_state_index_], curAmbiguityState());//拒绝伪距测量中的离群值
rejectDopplerOutlier(states_[latest_state_index_]);//拒绝Doppler测量中的离群值
rejectPhaserangeOutlier(states_[latest_state_index_], curAmbiguityState());//拒绝相位测量中的离群值
}
// Check if we rejected too many GNSS residuals检查是否拒绝了过多的GNSS残差
double ratio_pseudorange = n_pseudorange == 0.0 ? 0.0 : 1.0 - //计算伪距残差比例，用于评估是否拒绝了过多的GNSS残差
getDivide(numPseudorangeError(states_[latest_state_index_]), n_pseudorange);
double ratio_phaserange = n_phaserange == 0.0 ? 0.0 : 1.0 - //计算相位残差比例
getDivide(numPhaserangeError(states_[latest_state_index_]), n_phaserange);
double ratio_doppler = n_doppler == 0.0 ? 0.0 : 1.0 - //计算Doppler残差比例
getDivide(numDopplerError(states_[latest_state_index_]), n_doppler);
const double thr = gnss_base_options_.diverge_max_reject_ratio;//从配置中获取最大拒绝比例的阈值
if (isGnssGoodObservation() && //检查观测是否良好且拒绝比例是否超过阈值
(ratio_pseudorange > thr || ratio_phaserange > thr || ratio_doppler > thr)) {
num_cotinuous_reject_gnss_++;//如果观测良好且至少有一个拒绝比例超过了阈值，则增加连续拒绝计数
}
else num_cotinuous_reject_gnss_ = 0;
if (num_cotinuous_reject_gnss_ > //如果连续拒绝计数超过了配置中定义的最小允许连续拒绝值，则认为估计器发散
gnss_base_options_.diverge_min_num_continuous_reject) {
LOG(WARNING) << "Estimator diverge: Too many GNSS outliers rejected!";
status_ = EstimatorStatus::Diverged;//设置状态为发散
num_cotinuous_reject_gnss_ = 0;//重置连续拒绝计数
}
// Ambiguity resolution模糊度解算
for (size_t i = latest_state_index_; i < states_.size(); i++) {//遍历最新状态到最后的状态
states_[i].status = GnssSolutionStatus::Float;//初始化状态为浮点解
}
if (rtk_options_.use_ambiguity_resolution) {//检查是否启用模糊度解算
// get covariance of ambiguities获取模糊度的协方差矩阵
Eigen::MatrixXd ambiguity_covariance;
if (estimateAmbiguityCovariance(//获取当前状态（最新状态）的模糊度协方差矩阵
states_[latest_state_index_], ambiguity_covariance))
{
// solve解算模糊度
AmbiguityResolution::Result ret = ambiguity_resolution_->solveRtk(
states_[latest_state_index_].id, curAmbiguityState().ids,
ambiguity_covariance, gnss_measurement_pairs_.back());
if (ret == AmbiguityResolution::Result::NlFix) {//如果模糊度解算成功
for (size_t i = latest_state_index_; i < states_.size(); i++) {
states_[i].status = GnssSolutionStatus::Fixed;//将从最新状态到最后一个状态的所有状态的解状态更新为已固定解状态
}
}
}
}
//具有良好观测的情况下，检查是否连续无法修复模糊度
// Check if we continuously cannot fix ambiguity, while we have good observations
if (rtk_options_.use_ambiguity_resolution) {//检查是否启用模糊度解算
const double thr = gnss_base_options_.good_observation_max_reject_ratio;//从配置中获取用于判断良好观测比例的阈值
if (isGnssGoodObservation() && ratio_pseudorange < thr && //如果观测良好且伪距、相位距离和多普勒的拒绝比例都低于阈值
ratio_phaserange < thr && ratio_doppler < thr) {
if (curState().status != GnssSolutionStatus::Fixed) num_continuous_unfix_++;//如果当前状态未被固定解，则增加连续无法解决模糊度的计数
else num_continuous_unfix_ = 0; //否则，重置计数
}
else num_continuous_unfix_ = 0;
if (num_continuous_unfix_ > //连续无法解决模糊度的计数超过了配置中定义的最小允许连续无法解决值
gnss_base_options_.reset_ambiguity_min_num_continuous_unfix) {
LOG(INFO) << "Continuously unfix under good observations. Clear current ambiguities.";//记录信息
resetAmbiguityEstimation();//重置当前模糊度的估计
ambiguity_covariance_estimator_->resetAmbiguityEstimation();
num_continuous_unfix_ = 0;//重置连续无法解决模糊度的计数
}
}
}
// Image processes
if (new_state_type == IdType::cPose) {//如果最新的状态是相机测量结果
// update landmarks to frontend更新前端的地标点
updateLandmarks();
// update states to frontend更新前端的状态
updateFrameStateToFrontend(states_[latest_state_index_], curFrame());
// reject landmark outliers拒绝视觉测量的离群值
size_t n_reprojection = numReprojectionError(curFrame());//获取视觉测量的重投影误差数量
rejectReprojectionErrorOutlier(curFrame());//拒绝视觉测量的重投影误差的离群值
// check if we rejected too many reprojection errors检查是否拒绝了过多的重投影误差
double ratio_reprojection = n_reprojection == 0.0 ? 0.0 : 1.0 - //计算重投影误差的拒绝比例
getDivide(numReprojectionError(curFrame()), n_reprojection);
if (ratio_reprojection > visual_base_options_.diverge_max_reject_ratio) {
num_cotinuous_reject_visual_++;//如果拒绝比例超过阈值，则增加连续拒绝计数
}
else num_cotinuous_reject_visual_ = 0;//否则，重置连续拒绝计数
if (num_cotinuous_reject_visual_ > //如果连续拒绝计数超过了配置中定义的最小允许连续拒绝值
visual_base_options_.diverge_min_num_continuous_reject) {
LOG(WARNING) << "Estimator diverge: Too many visual outliers rejected!";
status_ = EstimatorStatus::Diverged;//设置状态为发散
num_cotinuous_reject_visual_ = 0;//重置连续拒绝计数
}
}
// Apply marginalization根据最新状态的类型应用边缘化
marginalization(new_state_type);
// Shift memory for states and measurements调整内存以容纳新的状态和测量，并保持相应的窗口长度
if (new_state_type == IdType::gPose) {//对于gPose类型
gnss_measurement_pairs_.push_back(//添加一个新的GNSS测量对
std::make_pair(GnssMeasurement(), GnssMeasurement()));
ambiguity_states_.push_back(AmbiguityState());//添加一个新的模糊度状态
}//对于cPose类型，添加一个新的帧束
if (new_state_type == IdType::cPose) frame_bundles_.push_back(nullptr);
states_.push_back(State());
while (!visual_initialized_ && //如果视觉初始化尚未完成
states_.size() > rrr_options_.max_gnss_window_length_minor) {//GNSS状态的数量超过了配置中定义的最大GNSS窗口长度时
states_.pop_front();//从头部移除相应数量的GNSS状态、模糊度状态和GNSS测量对
ambiguity_states_.pop_front();
gnss_measurement_pairs_.pop_front();
}
// only keep frame measurement data for two epochs//仅保留两个历元的关键帧测量数据
while (frame_bundles_.size() > 2) frame_bundles_.pop_front();
// Apply ceres optimization设置Ceres Solver的优化选项，调用求解器进行优化，并根据需要更新协方差。
void EstimatorBase::optimize()
{
graph_->options.linear_solver_type = base_options_.solver_type;//设置线性求解器的类型
graph_->options.trust_region_strategy_type = base_options_.trust_region_strategy_type;//设置信任域策略的类型
graph_->options.num_threads = base_options_.num_threads;//设置用于优化的线程数
graph_->options.max_num_iterations = base_options_.max_iteration;//设置最大迭代次数
#ifdef NDEBUG //如果在编译时定义了 NDEBUG 宏，那么会启用一些调试设置。
graph_->options.max_solver_time_in_seconds = base_options_.max_solver_time;
#endif
if (base_options_.verbose_output) {
// graph_->options.minimizer_progress_to_stdout = true;
}
else {//设置日志类型为 SILENT（静默），并禁止向 stdout 打印优化器进度。
graph_->options.logging_type = ceres::LoggingType::SILENT;
graph_->options.minimizer_progress_to_stdout = false;
}
// call solver
graph_->solve();//调用求解器进行优化
// update covariance
if (base_options_.compute_covariance && can_compute_covariance_) {
updateCovariance(latestState());//更新协方差
}
}
在graph.h文件中找到如下：
// these are public for convenient manipulation
/// \brief Ceres options
ceres::Solver::Options options;
/// \brief Ceres optimization summary
ceres::Solver::Summary summary;
/// @brief Solve the optimization problem.
void solve()
{
Solve(options, problem_.get(), &summary);
}
可以看到调用了Ceres库的Solve()函数进行解算，这里可以参考这篇优秀文章对Solve()进行学习：
【SLAM】Ceres优化库超详细解析_ceres库-CSDN博客
在gnss_estimator_base.h/686行找到：
// Ambiguity resolution
std::unique_ptr<AmbiguityResolution> ambiguity_resolution_;
在ambiguity_resolution_differential.cpp 文件中找到：
/ Solve ambiguity for single differenced ambiguities (with double differenced measurements)
AmbiguityResolution::Result AmbiguityResolution::solveRtk(
const BackendId& epoch_id,
const std::vector<BackendId>& ambiguity_ids,
const Eigen::MatrixXd& ambiguity_covariance,
const std::pair<GnssMeasurement, GnssMeasurement>& measurements)
{
// Prepare data
// Shift storage
ambiguities_.push_back(std::vector<Spec>());//将新的空向量添加到存储模糊度的容器中
ambiguity_pairs_.push_back(std::vector<BsdPair>());//将新的空向量添加到存储模糊对容器中
ambiguity_lane_pairs_.push_back(std::vector<LanePair>());//将新的空向量添加到存储模糊通道对容器中
if (ambiguities_.size() > 2) {//如果存储的历元数量超过2个
ambiguities_.pop_front();//从容器的头部弹出最老的数据，以保持存储的历元数量不超过2个
ambiguity_pairs_.pop_front();
ambiguity_lane_pairs_.pop_front();
}
full_parameters_store_.clear();//清空用于存储完整参数的容器，以确保存储新的RTK解算数据之前不包含之前的数据
lane_groups_.clear();
// Get parameters
const GnssMeasurement& measurements_rov = measurements.first;//获取移动站的测量数据
const GnssMeasurement& measurements_ref = measurements.second;
std::unordered_map<size_t, size_t> ambiguity_index_map;//创建一个无序映射，用于存储模糊度ID到其在容器中的索引的映射关系
for (size_t i = 0; i < ambiguity_ids.size(); i++) {//对给定的模糊度ID列表进行循环遍历
char system = ambiguity_ids[i].gSystem();//获取系统标识符
std::string prn = ambiguity_ids[i].gPrn();//获取卫星prn号
int phase_id = ambiguity_ids[i].gPhaseId();//获取相位ID
if (!gnss_common::useSystem(options_.system_exclude, system)) continue;//检查是否应该排除当前系统,如果需要排除，则跳过当前模糊度的处理
if (!gnss_common::useSatellite(options_.satellite_exclude, prn)) continue;
if (!gnss_common::usePhase(options_.phase_exclude, system, phase_id)) continue;
const Eigen::Vector3d& position = measurements_rov.position;
const Eigen::Vector3d& sat_position =
measurements_rov.satellites.at(prn).sat_position;
double elevation = gnss_common::satelliteElevation(sat_position, position);//计算卫星的仰角
if (gnss_common::radToDegree(elevation) < options_.min_elevation) continue;//如果仰角小于最小仰角，则跳过当前模糊度
//将模糊度ID转换为整数作为参数块的ID
uint64_t id = ambiguity_ids[i].asInteger();
Spec ambiguity;
ambiguity.id = ambiguity_ids[i];
ambiguity.prn = prn;
CHECK(graph_->parameterBlockExists(id));//检查相应的参数块是否存在于图中
// parameter and residual block
ambiguity.parameter_block = graph_->parameterBlockPtr(id);//获取模糊度的参数块指针
ambiguity.value = *ambiguity.parameter_block->parameters();//获取模糊度的参数值
Graph::ResidualBlockCollection residuals = graph_->residuals(id);//获取图中与模糊度相关的残差块
CHECK(residuals.size() > 0);//确保至少存在一个残差块
// Check the number of phaserange errors. Ideally, for a non-reference satellite ambiguity
// parameter block, it only conresponds to one phaserange error block.
int num_phaserange_block = 0;
for (size_t r = 0; r < residuals.size(); ++r) {//统计与相位测量相关的残差块数量
if (residuals[r].error_interface_ptr->typeInfo() == ErrorType::kPhaserangeError ||
residuals[r].error_interface_ptr->typeInfo() == ErrorType::kPhaserangeErrorSD ||
residuals[r].error_interface_ptr->typeInfo() == ErrorType::kPhaserangeErrorDD) {
num_phaserange_block++;
}
}
if (num_phaserange_block == 1)//如果相位测量相关的残差块只有一个
for (size_t r = 0; r < residuals.size(); ++r) {
if (residuals[r].error_interface_ptr->typeInfo() == ErrorType::kPhaserangeError ||
residuals[r].error_interface_ptr->typeInfo() == ErrorType::kPhaserangeErrorSD ||
residuals[r].error_interface_ptr->typeInfo() == ErrorType::kPhaserangeErrorDD) {
ambiguity.residual_block = residuals[r];//获取模糊度对应的残差块
break;
}
}
// wavelength
const Satellite& satellite = measurements_rov.satellites.at(prn);//获取移动站测量数据中给定卫星的观测数据
for (auto obs : satellite.observations) {//遍历给定卫星的观测数据
auto& observation = obs.second;
if (gnss_common::getPhaseID(system, obs.first) == phase_id) {///找到与目标相位ID匹配的观测数据
ambiguity.wavelength = observation.wavelength;//存储观测数据的波长
}
}
// elevation angle for reference satellite selection
ambiguity.elevation = elevation;//存储卫星的仰角
curAmbs().push_back(ambiguity);//将当前计算的模糊度信息添加到模糊度集合中
ambiguity_index_map.insert(std::make_pair(curAmbs().size() - 1, i));
}//将当前计算的模糊度在模糊度集合中的索引与原始模糊度ID的索引之间建立映射关系
// Get covariance of ambiguity
CHECK(ambiguity_covariance.cols() == ambiguity_ids.size());//确保模糊度的协方差矩阵的列数等于模糊度的数量
const size_t ambiguity_size = curAmbs().size();//获取模糊度集合的大小
ambiguity_covariance_.resize(ambiguity_size, ambiguity_size);//将模糊度的协方差矩阵的大小调整为模糊度集合的大小
for (size_t i = 0; i < ambiguity_size; i++) {//遍历当前模糊度集合
for (size_t j = 0; j < ambiguity_size; j++) {
const size_t raw_i = ambiguity_index_map.at(i);
const size_t raw_j = ambiguity_index_map.at(j);
ambiguity_covariance_(i, j) = ambiguity_covariance(raw_i, raw_j);//将原始模糊度协方差矩阵中的值复制到当前模糊度的协方差矩阵中
}
curAmbs()[i].std = sqrt(ambiguity_covariance_(i, i));//计算模糊度的标准差
}
// Collect all parameter blocks
Graph::ParameterBlockCollection parameters = graph_->parameters();//获取图优化中所有参数块的集合
for (size_t p = 0; p < parameters.size(); p++) {//遍历所有参数块
Parameter parameter;
parameter.id = BackendId(parameters[p].first);//存储参数块的ID
parameter.size = parameters[p].second->dimension();//存储参数块的大小
parameter.minimal_size = parameters[p].second->minimalDimension();//存储参数块的最小尺寸
parameter.value = Eigen::Map<Eigen::VectorXd>(//存储参数块的值
parameters[p].second->parameters(), parameter.size);
parameter.handle = parameters[p].second;
if (full_parameters_store_.size() == 0) {//如果参数块集合为空，表示当前是第一个参数块
parameter.covariance_start_index = 0;//参数块协方差矩阵的起始索引为0
}
else {//否则，通过上一个参数块的信息确定当前参数块的协方差矩阵的起始索引
Parameter& last_parameter = full_parameters_store_.back();
parameter.covariance_start_index =
last_parameter.covariance_start_index + last_parameter.minimal_size;
}
full_parameters_store_.push_back(parameter);//将当前参数块的信息添加到参数块集合中
}
// Sovle ambiguity
// Apply Between-Satellite-Difference (BSD) and lane combination
formSatellitePairRtk();//应用卫星间差分和通道组合
// Sort lanes to groups对形成的通道进行分组
groupingSatellitePair();
if (lane_groups_.size() == 0) return Result::NoFix;//如果分组数量为0，表示没有可用的通道组合
// Try fix lanes尝试固定通道
int num_success_uwl = 0, num_candidate_uwl = 0;
int num_success_wl = 0, num_candidate_wl = 0;
int num_success_nl = 0, num_candidate_nl = 0;
bool has_uwl = false, has_wl = false;
for (int i = lane_groups_.size() - 1; i >= 0; i--) {//从后往前遍历通道组合
LaneType type = curAmbLanePairs().at(lane_groups_.at(i).front()).laneType();//获取当前通道组合的类型
double min_percentage_fixation;
bool use_rounding = false;
if (type == LaneType::UWL) {//根据通道组合的类型设置相应的最小固定百分比和是否使用四舍五入
min_percentage_fixation = options_.min_percentage_fixation_uwl;
use_rounding = false;
has_uwl = true;
}
else if (type == LaneType::WL) {
min_percentage_fixation = options_.min_percentage_fixation_wl;
use_rounding = false;
has_wl = true;
}
else {
min_percentage_fixation = options_.min_percentage_fixation_nl;
use_rounding = false;
}
int num_fixed = trySolveLanes(//尝试对当前通道组合进行解算，并返回成功固定的通道数量
lane_groups_.at(i), min_percentage_fixation, use_rounding);
//根据通道组合的类型统计成功固定的通道数量和候选通道数量
if (type == LaneType::UWL) {
num_success_uwl += num_fixed;
num_candidate_uwl += lane_groups_.at(i).size();
}
else if (type == LaneType::WL) {
num_success_wl += num_fixed;
num_candidate_wl += lane_groups_.at(i).size();
}
else {
num_success_nl += num_fixed;
num_candidate_nl += lane_groups_.at(i).size();
}
}
// Check if no valid fixation如果没有成功固定的通道，则返回 Result::NoFix 表示无法解算
if (num_success_nl == 0 && num_success_wl == 0 &&
num_success_uwl == 0) return Result::NoFix;
// Apply in graph and check residual
double range_cost_before = computeRangeCost(epoch_id);//计算在应用通道固定前和后的残差
graph_->options.max_num_iterations = 1;//将优化步数限制为1，以确保只应用一次优化
graph_->options.logging_type = ceres::LoggingType::SILENT;
graph_->options.minimizer_progress_to_stdout = false;
graph_->solve();//解算图
double range_cost = computeRangeCost(epoch_id);//计算新的残差
const double tolerance = 0.01;
if (range_cost > range_cost_before + tolerance) {//如果新的残差超过一定的阈值，认为通道固定无效
LOG(INFO) << "Invalid ambiguity resolution: Total cost changes from "
<< std::scientific << std::setprecision(3)
<< range_cost_before << " to " << range_cost << ".";
setGraphParameters(full_parameters_store_);//将图参数还原
eraseAmbiguityResidualBlocks(curAmbLanePairs());//删除与通道组相关的残差块
return Result::NoFix;
}
// Check status
if (num_success_nl > 0) return Result::NlFix;//如果成功固定了窄巷通道，则返回 Result::NlFix
else if (num_success_wl > 0) return Result::WlFix;//如果成功固定了宽巷通道，则返回 Result::WlFix
return Result::NoFix;
}
根据传参类型边缘化
// Marginalization
bool RtkImuCameraRrrEstimator::marginalization(const IdType& type)
{
if (type == IdType::cPose) return frameMarginalization();//如果类型为cPose，则边缘化帧状态
else if (type == IdType::gPose) return gnssMarginalization();//如果类型为gPose，则边缘化GNSS状态
else return false;
}
// Marginalization
bool RtkImuCameraRrrEstimator::marginalization(const IdType& type)
{
if (type == IdType::cPose) return frameMarginalization();//如果类型为cPose，则边缘化关键帧状态
else if (type == IdType::gPose) return gnssMarginalization();//如果类型为gPose，则边缘化GNSS状态
else return false;
}
// Marginalization when the new state is a frame state
bool RtkImuCameraRrrEstimator::frameMarginalization()
{
// Check if we need marginalization
if (isFirstEpoch()) return true;//如果这是第一个历元，则不需要边缘化
// Make sure that only current state can be non-keyframe确保只有当前状态可以是非关键帧
for (int i = 0; i < states_.size() - 1; i++) {//遍历之前时刻的状态
if (states_[i].id.type() != IdType::cPose) continue;//如果状态不是相机类型，则跳过
if (!states_[i].is_keyframe) {//如果状态不是关键帧
eraseReprojectionErrorResidualBlocks(states_[i]);//删除重投影误差残差块
eraseImuState(states_[i]);//删除IMU状态
i--;
}
}
// If current frame is a keyframe. Marginalize the oldest keyframe and corresponding
// IMU and GNSS states out. And sparsify GNSS states.
if (states_[latest_state_index_].is_keyframe &&//如果当前帧是关键帧，普通帧之前已经被删除
sizeOfKeyframeStates() > rrr_options_.max_keyframes) {//如果关键帧数量大于配置中定义的阈值
// Erase old marginalization item删除旧的边缘化项，失败则报错
if (!eraseOldMarginalization()) return false;
// Add marginalization items添加边缘化项
// marginalize oldest keyframe state边缘化最老的关键帧状态
bool reached_first_keyframe = false;//用于标记是否已经到达第一个关键帧
State margin_keyframe_state;//用于保存将要被边缘化的关键帧状态
int n_state = 0, n_frame = 0, n_gnss = 0;
for (auto it = states_.begin(); it != states_.end();) {//遍历所有状态
State& state = *it;
// reached the first keyframe
if (state.is_keyframe) reached_first_keyframe = true;
// mark marginalized keyframe state如果当前状态是关键帧，将其保存，作为将要被边缘化的关键帧状态
if (state.is_keyframe) margin_keyframe_state = state;
// add margin blocks
// Keyframe, we add the reprojection errors latter.//对关键帧，稍后添加重投影误差
if (state.is_keyframe) {
addImuStateMarginBlock(state);//添加IMU状态边缘化块
addImuResidualMarginBlocks(state);//添加IMU残差边缘化块
}
// GNSS state that not yet been marginalized by GNSS steps.尚未边缘化GNSS步骤的GNSS状态
else {
auto it_ambiguity = ambiguityStateAt(state.timestamp);//根据GNSS状态的时间戳获取相应的模糊状态的迭代器
if (it_ambiguity != ambiguity_states_.end()) {//如果存在模糊状态
addAmbiguityMarginBlocksWithResiduals(*it_ambiguity);//添加与模糊状态相关的边缘化项
}
else {//如果不存在模糊状态
addGnssLooseResidualMarginBlocks(state);//添加与GNSS松组合残差边缘化项
}
addImuStateMarginBlock(state);//添加与IMU状态相关的边缘化项
addImuResidualMarginBlocks(state);//添加与IMU残差相关的边缘化项
addFrequencyMarginBlocksWithResiduals(state);//添加频率边缘化项
addGnssResidualMarginBlocks(state);//添加与GNSS相关的边缘化项
// erase ambiguity state and GNSS measurements删除模糊度状态和GNSS测量值
if (it_ambiguity != ambiguity_states_.end()) {
ambiguity_states_.erase(it_ambiguity);//删除模糊度状态
}
if (gnssMeasurementPairAt(state.timestamp) != gnss_measurement_pairs_.end()) {
gnss_measurement_pairs_.erase(gnssMeasurementPairAt(state.timestamp));//删除GNSS测量值
}
}
// Erase state
it = states_.erase(it);//删除it指向的状态，并返回指向被删除状态之后的迭代器
n_state++;//增加状态计数器
if (reached_first_keyframe) break;//如果已经到达第一个关键帧，就跳出循环
}
// landmarks
eraseEmptyLandmarks();//删除空的路标
addLandmarkParameterMarginBlocksWithResiduals(margin_keyframe_state);//添加路标参数边缘化项与残差相关
// Apply marginalization and add the item into graph
bool ret = applyMarginalization();//应用边缘化，并添加到图
return ret;
}
return true;
}
// Apply new marginalization应用新的边缘化
bool EstimatorBase::applyMarginalization()
{
// Check if there are any residuals we forgot to add检查是否有遗漏的残差块
for (auto id : marginalization_parameter_ids_) {//对于每个边缘化的参数块
auto residuals = graph_->residuals(id.asInteger());//检查图中是否有与之相关的残差块
for (auto residual : residuals) {//将这些残差块添加到边缘化误差对象中
marginalization_error_->addResidualBlock(residual.residual_block_id);
}
}
// Apply marginalization应用边缘化
std::vector<uint64_t> parameter_blocks_to_be_marginalized;
for (auto margin_id : marginalization_parameter_ids_) {
parameter_blocks_to_be_marginalized.push_back(margin_id.asInteger());//将边缘化的参数块id添加到要被边缘化的参数块id列表中
}
marginalization_error_->marginalizeOut(parameter_blocks_to_be_marginalized,//边缘化参数块
marginalization_keep_parameter_blocks_);
// update error computation如果存在要边缘化的参数块，那么更新计算误差
if(parameter_blocks_to_be_marginalized.size() > 0) {
marginalization_error_->updateErrorComputation();
}
// add the marginalization term again再次添加边缘化项
if(marginalization_error_->num_residuals()==0)
{//如果边缘化误差对象中没有残差，则将其重置为nullptr
marginalization_error_.reset();
}
if (marginalization_error_)//边缘化误差对象存在
{//获取参数块的指针并将其传递给图对象，以添加一个新的残差块。
std::vector<std::shared_ptr<ParameterBlock> > parameter_block_ptrs;//初始化参数块的指针列表
marginalization_error_->getParameterBlockPtrs(parameter_block_ptrs);//获取参数块的指针列表
marginalization_residual_id_ = graph_->addResidualBlock(//添加一个新的残差块
marginalization_error_, nullptr, parameter_block_ptrs);
CHECK(marginalization_residual_id_)
<< "could not add marginalization error";
if (!marginalization_residual_id_) return false;
}
return true;
}
// Marginalization when the new state is a GNSS state
bool RtkImuCameraRrrEstimator::gnssMarginalization()
{
// Check if we need marginalization
if (isFirstEpoch()) return true;//如果这是第一个历元，则不需要边缘化
// Visual not initialzed, only handle GNSS and INS
if (!visual_initialized_) {//如果视觉未初始化，说明系统只处理GNSS和INS部分
// Check if we need marginalization 检查状态数量是否小于给定的最大GNSS窗口长度
if (states_.size() < rrr_options_.max_gnss_window_length_minor) {
return true;//如果是，则返回true，表示不需要进行边缘化
}
// Erase old marginalization item删除旧的边缘化项
if (!eraseOldMarginalization()) return false;
// Add marginalization items
// IMU states and residuals
addImuStateMarginBlockWithResiduals(oldestState());//为最老的IMU状态添加边缘化项和残差
// ambiguity
addAmbiguityMarginBlocksWithResiduals(oldestAmbiguityState());//为最老的模糊度状态添加边缘化项和残差
// frequency
addFrequencyMarginBlocksWithResiduals(oldestState());//为最老的状态添加与频率相关的边缘化项和残差
// Apply marginalization and add the item into graph
bool ret = applyMarginalization();
return ret;
}
// Visual initialized, handle all the sensors
else {//视觉初始化，处理所有传感器
// Sparsify GNSS states
sparsifyGnssStates();//对GNSS状态进行稀疏化处理
// Marginalize the GNSS states that in front of the oldest keyframe边缘化在最老的关键帧之前的GNSS状态
// We do this at both here and the visual margin step to smooth computational load//我们在这里和视觉边缘化步骤都这样做，以平滑计算负载
for (auto it = states_.begin(); it != states_.end();) {
State& state = *it;//获取当前迭代的状态
// reached the oldest keyframe
if (state.is_keyframe) break;//如果当前状态是关键帧，则跳出循环
// check if GNSS state
if (state.id.type() != IdType::gPose) continue;//如果当前状态不是GNSS状态，则跳过
// Erase old marginalization item删除旧的边缘化项
if (!eraseOldMarginalization()) return false;
// margin
auto it_ambiguity = ambiguityStateAt(state.timestamp);//获取GNSS状态的时间戳相应的模糊状态的迭代器
if (it_ambiguity != ambiguity_states_.end()) {
addAmbiguityMarginBlocksWithResiduals(*it_ambiguity);//添加与模糊度状态相关的边缘化项和残差
}
else {//如果没有找到相应的模糊状态
addGnssLooseResidualMarginBlocks(state);//添加与GNSS松组合残差相关的边缘化项
}
addImuStateMarginBlock(state);//添加与IMU状态相关的边缘化项
addImuResidualMarginBlocks(state);//添加与IMU残差相关的边缘化项
addFrequencyMarginBlocksWithResiduals(state);//添加与频率相关的边缘化项
addGnssResidualMarginBlocks(state);//添加与GNSS相关的边缘化项
// erase ambiguity state and GNSS measurements
if (it_ambiguity != ambiguity_states_.end()) {//如果找到相应的模糊状态
ambiguity_states_.erase(it_ambiguity);//删除模糊度状态
}
if (gnssMeasurementPairAt(state.timestamp) != gnss_measurement_pairs_.end()) {
gnss_measurement_pairs_.erase(gnssMeasurementPairAt(state.timestamp));//删除GNSS测量值
}
// Erase state
it = states_.erase(it);//删除it指向的状态，并返回指向被删除状态之后的迭代器
// just handle one in one time
bool ret = applyMarginalization();//应用边缘化，并添加到图
return ret;
}
}
return true;
}
// Sparsify GNSS states to bound computational load对GNSS状态进行稀疏化处理，以限制计算负载
void RtkImuCameraRrrEstimator::sparsifyGnssStates()
{
// Check if we need to sparsify
std::vector<BackendId> gnss_ids;//存储GNSS状态的ID
std::vector<int> num_neighbors;//存储每个GNSS状态相邻GNSS状态的数量
int max_num_neighbors = 0;
for (int i = 0; i < states_.size(); i++) {//遍历所有状态
if (states_[i].id.type() != IdType::gPose) continue;//如果当前状态不是GNSS状态，则跳过
gnss_ids.push_back(states_[i].id);//将GNSS状态的ID添加到gnss_ids中
// find neighbors
num_neighbors.push_back(0);//初始化当前状态相邻状态的数量为0
int idx = num_neighbors.size() - 1;//记录当前状态在num_neighbors中的索引
if (i - 1 >= 0) for (int j = i - 1; j >= 0; j--) {//如果当前状态前面还有GNSS状态，就计算前面GNSS状态的数量
if (states_[j].id.type() != IdType::gPose) break;//如果前面状态不是GNSS状态，则跳出循环
num_neighbors[idx]++;
} //如果当前状态后面还有GNSS状态，就计算后面GNSS状态的数量
if (i + 1 < states_.size()) for (int j = i; j < states_.size(); j++) {
if (states_[j].id.type() != IdType::gPose) break;
num_neighbors[idx]++;
}
if (max_num_neighbors < num_neighbors[idx]) {//如果当前状态相邻状态的数量大于最大数量
max_num_neighbors = num_neighbors[idx];//则更新最大数量
}
}//如果GNSS状态的数量不超过最大关键帧数，就不进行稀疏化，直接返回
if (gnss_ids.size() <= rrr_options_.max_keyframes) return;
// Erase some GNSS states
// The states with most neighbors will be erased first 具有最多相邻的GNSS状态将首先被删除
int num_to_erase = gnss_ids.size() - rrr_options_.max_keyframes;//计算需要删除的GNSS状态数量
std::vector<BackendId> ids_to_erase;//存储将要删除的GNSS状态的ID
for (int m = max_num_neighbors; m >= 0; m--) {//对最多相邻GNSS状态数量进行逆序遍历
for (size_t i = 0; i < num_neighbors.size(); i++) {//遍历所有GNSS状态
if (num_neighbors[i] != m) continue;//如果当前状态相邻GNSS状态数量不等于m，则跳过
if (i == 0) continue; // in case the first one connects to margin block //如果当前状态是第一个连接边缘化项的GNSS状态，则跳过
ids_to_erase.push_back(gnss_ids[i]);//添加需要删除的GNSS状态的ID
if (ids_to_erase.size() >= num_to_erase) break;//如果需要删除的GNSS状态数量大于等于需要删除的数量，则跳出循环
}
if (ids_to_erase.size() >= num_to_erase) break;
}
CHECK(ids_to_erase.size() >= num_to_erase);// 确保已选择足够的状态以满足删除数量的要求
for (int i = 0; i < states_.size(); i++) {//遍历所有状态
if (std::find(ids_to_erase.begin(), ids_to_erase.end(), states_[i].id)
== ids_to_erase.end()) {
continue;//如果当前状态ID不在ids_to_erase中，则继续遍历
}
State& state = states_[i];//否则存储状态
auto it_ambiguity = ambiguityStateAt(state.timestamp);//存储模糊度状态
// the first one may be connected with margin error
if (it_ambiguity == ambiguity_states_.begin()) continue;//第一个状态是连接边缘化误差的，则跳过
eraseGnssMeasurementResidualBlocks(state);//删除GNSS测量值残差块
eraseFrequencyParameterBlocks(state);//删除频率参数块
// we may failed to find corresponding ambiguity state because the GNSS state can be
// loosely coupled state during initialization.
if (it_ambiguity != ambiguity_states_.end()) {//如果模糊度状态不为空
eraseAmbiguityParameterBlocks(*it_ambiguity);//删除模糊度参数块
ambiguity_states_.erase(it_ambiguity);//删除模糊度状态
}
else {
eraseGnssLooseResidualBlocks(state);//删除松组合GNSS测量值残差块
}
eraseImuState(state);//删除IMU状态
//如果该状态的GNSS测量对不为空
if (gnssMeasurementPairAt(state.timestamp) != gnss_measurement_pairs_.end()) {
gnss_measurement_pairs_.erase(gnssMeasurementPairAt(state.timestamp));//删除GNSS测量对
}
i--;
}
}
chat-gpt、文心一言
博主：