本文主要是介绍GICI-rtk/imu/camera紧组合代码学习，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
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_.coordinateif (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 solutionelse if (measurement.solution) {position_ecef = measurement.solution->coordinate->convert(measurement.solution->pose.getPosition(), GeoType::ENU, GeoType::ECEF);}// no where to get coordinate zeroelse {return false;}// set coordinatesolution_.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 neededif (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 measurementif (estimator_->addMeasurement(measurement)) {// solve 如果估计有效，将 is_updated 标志设置为 trueif (estimator_->estimate()) is_updated = true;//std::shared_ptr<EstimatorBase> estimator_;在estimating.h文件中// check if estimator validif (estimator_->getStatus() == EstimatorStatus::Diverged) {// reset estimator 如果估计发散，重置估计器并将 is_updated 标志设置为 falseLOG(WARNING) << "Reset estimator because it is diverge!";resetProcessors();is_updated = false;}// log intermediate dataestimator_->logIntermediateData();}// Backend updated更新结果和初始状态if (is_updated)//估计有效{// update flagbackend_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 measurementif (estimator_->addMeasurement(measurement)) {// solve 如果估计有效，将 is_updated 标志设置为 trueif (estimator_->estimate()) is_updated = true;//std::shared_ptr<EstimatorBase> estimator_;在estimating.h文件中// check if estimator validif (estimator_->getStatus() == EstimatorStatus::Diverged) {// reset estimator 如果估计发散，重置估计器并将 is_updated 标志设置为 falseLOG(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 : **中的成员函数。
一、RtkImuCameraRrrEstimator::addMeasurement（）
// GNSS/IMU initializationif (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 estimatorsetInitializationResult(gnss_imu_initializer_);}return false;}
其中的gnss_imu_initializer_ 为指向类GnssImuInitializer 的共享指针，具体可在gnss_imu_initializer.h文件中查看。
// Add GNSSif (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 localGnssMeasurement 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);}
1、gnss_imu_initializer_->addMeasurement()
在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通过解决方案测量添加GNSSif (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通过原始测量添加GNSSif (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;
}
2、gnss_imu_initializer_->estimate()
// 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;
}
optimize()
找到GnssImuInitializer 的父类MultisensorInitializerBase
// Estimator
class GnssImuInitializer : public MultisensorInitializerBase, public GnssLooseEstimatorBase,public ImuEstimatorBase
而类MultisensorInitializerBase 又继承自类EstimatorBase 并找到如下：
protected:// Apply ceres optimizationvirtual 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;
#endifif (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 solvergraph_->solve();//调用求解器进行优化// update covariance if (base_options_.compute_covariance && can_compute_covariance_) {updateCovariance(latestState());//更新协方差}
}
其中的核心graph_ 为指向类Graph 的智能指针，此处略过，后期在详细介绍。
3、setInitializationResult()
// 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 lengthImuMeasurements imu_measurements;std::deque<GnssSolution> gnss_measurement_temp; // we do not use thisgnss_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());//添加新的状态Stategnss_measurement_pairs_.resize(states_.size());//调整大小以匹配新的状态数量ambiguity_states_.resize(states_.size());frame_bundles_.push_back(nullptr);//添加空指针
}
GnssImuInitializer::arrangeToEstimator
//将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 residualsaddImuStateMarginBlockWithResiduals(state);//为姿态、速度、偏置参数添加边缘化块和对应的残差}// Apply marginalization and add the item into graphapplyMarginalization();//应用边缘化操作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;
}
EstimatorBase::applyMarginalization()
-
检查是否有遗漏的残差块:
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表示边缘化成功应用。
4、 addImuMeasurement()
在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。
5、ambiguity_covariance_estimator_->addMeasurement()
在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_
meausrement_align_.add()
// Set measurementsinline 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);将测量值添加到参考站观测值队列中}}
meausrement_align_.get()
// 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队列中的第一个测量值赋值给rovmeasurement_rov_.pop_front();//将Rover队列中的第一个测量值弹出// get the nearest timestampdouble 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是否为-1ref = measurement_ref_.at(index);//将参考站队列指定时间戳的值赋值给ref// pop all in front of thisdouble 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;}
6、ambiguity_covariance_estimator_->estimate()
// 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;//否则，将计数器重置为0if (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 resolutioncurState().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);// solveAmbiguityResolution::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 observationsif (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 informationif (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 marginalizationmarginalization();// Shift memory for states and measurements将状态和测量值的内存进行迁移shiftMemory();return true;
}
7、meausrement_align_.add()
参考一、5的meausrement_align_.add()
8、meausrement_align_.get()
参考一、5的meausrement_align_.get()
9、addGnssMeasurementAndState()
// Add GNSS measurements and state
bool RtkImuCameraRrrEstimator::addGnssMeasurementAndState(const GnssMeasurement& measurement_rov, const GnssMeasurement& measurement_ref)
{// Get prior states获取先验坐标转到 ECEFEigen::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 phasegnss_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 detectionif (!isFirstEpoch()) {//不是第一历元cycleSlipDetectionSD(lastGnssRov(), lastGnssRef(), curGnssRov(), curGnssRef(), gnss_base_options_.common);}// Add parameter blocksdouble timestamp = curGnssRov().timestamp;//获取当前流动站量测值时间戳// pose and speed and bias blockconst int32_t bundle_id = curGnssRov().id;//获取当前流动站IDBackendId pose_id = createGnssPoseId(bundle_id);//创建GNSS坐标系IDsize_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 stepCHECK(gnss_extrinsics_id_.valid());//检查GNSS外参是否有效。通常，这应该在初始化步骤中添加外参// ambiguity blocksaddSdAmbiguityParameterBlocks(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 blocksaddDopplerResidualBlocks(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// frequencyaddRelativeFrequencyResidualBlock(lastGnssState(), states_[index]);// ambiguityaddRelativeAmbiguityResidualBlock(lastGnssRov(), curGnssRov(), lastAmbiguityState(), curAmbiguityState());}// ZUPT添加零速度更新（ZUPT）残差块，用于利用IMU测量的零速度信息addZUPTResidualBlock(states_[index]);// Car motion如果启用了车辆运动的模型，添加车辆运动的相关约束，包括航向角度和非完整约束if (imu_base_options_.car_motion) {// heading measurement constraintaddHMCResidualBlock(states_[index]);// non-holonomic constraintaddNHCResidualBlock(states_[index]);}// Compute DOP计算位置精度因子updateGdop(curGnssRov(), code_index_pairs);return true;
}
gnss_common::rearrangePhasesAndCodes()
在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 biasfor (auto& sat : measurement.satellites) {//遍历卫星std::string prn = sat.first;//获取卫星的prnchar 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;//获取观测值对应的codeObservation observation = it->second;//获取观测主体int phase_id = getPhaseID(system, code);//获取观测相位IDint 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 codeelse {//排列为默认码double bias = code_bias->getCodeBias(prn, code, accept_coarse);//获取码偏double default_bias = code_bias->getCodeBias(prn, default_code, accept_coarse);// do not have code biasif (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 arrangeelse {// passcan_add = false;}}// use code bias to arrangeelse {observation.pseudorange += bias - default_bias;//将观测值中的伪距加上偏移arranged_observation = std::make_pair(default_code, observation);//将观测值放入整理后的观测值映射中}}// add to observationsif (can_add) {if (arranged_observations.find(default_code) == arranged_observations.end()) {//如果整理后的观测值映射中不存在该codearranged_observations.insert(arranged_observation);//将整理后的卫星观测值放入整理后的卫星观测值映射中}}}satellite.observations = arranged_observations;}
}
gnss_common::formPhaserangeDDPair()
该函数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 pairGnssMeasurementSDIndexPairs 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为相位IDstd::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;//获取观测卫星的PRNauto it = prn_to_number_phases.find(prn);//通过prn(key键)找到相应的映射对if (it == prn_to_number_phases.end()) {//如果迭代器找不到，生成新的prn对，值设为1prn_to_number_phases.insert(std::make_pair(prn, 1));}else it->second++;//否则值加1prn_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);//获取卫星系统相位IDprn_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值为0for (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为相位IDif (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;//获取prnstd::string prn_base = system_to_base_prn.at(system);//获取当前系统的基准卫星PRNif (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);//获取基准卫星相位的IDint phase_id = getPhaseID(system, sd_pairs[i].rov.code_type);//获取当前卫星相位的IDif (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;}}}
formPhaserangeSDPair
函数在在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)));}}}
gnss_common::formPseudorangeDDPair()
该函数formPseudorangeDDPair() 存在重载，此处在gnss_common.cpp/522行调用。伪距双差与相位双差的逻辑大致一样
cycleSlipDetectionSD()
周跳探测函数，没找到在何处，理论上来说应该在gnss_estimator_bash类中。
insertImuState()
在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,//当前观测值时间戳和后端idconst 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 memoryif (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=后端idstate.id_in_graph = states_[overlap_index].id_in_graph;//新状态图优化的id=重叠状态图优化的idstate.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) {//如果如果之前未记录过这个图优化IDState::overlaps.insert(std::make_pair(state.id_in_graph, states_[overlap_index]));//插入重叠状态的图优化ID}//否则插入新状态的图优化IDState::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 || //如果最新有效状态的时间戳为0checkLessEqual(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 themelse {// 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;//设置后端IDstate.id_in_graph = backend_id;//设置图优化IDauto 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();
10、visualInitialization（）
// Visual initialization
bool RtkImuCameraRrrEstimator::visualInitialization(const FrameBundlePtr& frame_bundle)
{// store posesdo_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 keyframesif (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 posesstd::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 landmarksfeature_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 flagvisual_initialized_ = true;do_not_remove_imu_measurements_ = false;can_compute_covariance_ = true;return true;
}
feature_handler_->initializeLandmarks()
在feature_handler.cpp/493
// Initialize landmarks
void FeatureHandler::initializeLandmarks(const FramePtr& keyframe)
{const FramePtr& frame = keyframe;if (!frame->isKeyframe()) return;//检查是否是关键帧mutex_.lock();//确保在多线程下安全访问数据// Initialize landmarksfor (size_t i = 0; i < frame->numFeatures(); i++) {//遍历关键帧的所有特征点auto& landmark = frame->landmark_vec_[i];//获取特征点对应的路标点CHECK(landmark != nullptr);// already initializedif (isSeed(frame->type_vec_[i])) continue;//如果路标点初始化过，则跳过// rejected by bundle adjuster 跳过被标记为outlier的特征点if (frame->type_vec_[i] == FeatureType::kOutlier) continue;// get the last keyframeFramePtr 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 disparitydouble 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 typefor (auto obs : landmark->obs_) {//遍历当前路标点的观测值if (FramePtr f = obs.frame.lock()) {//获取观测值的帧changeFeatureTypeToSeed(f->type_vec_[obs.keypoint_index_]);//将观测值对应的特征点类型设置为已初始化}}} mutex_.unlock();
}
11、addImageMeasurementAndState（）
// 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 epochCHECK(!isFirstEpoch());//检查是否已经初始化。如果已经初始化，那么应该不处于第一个时刻// Check frequencyif (!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 blocksdouble timestamp = curFrame()->getTimestampSec();//获取当前帧的时间戳// pose and speed and bias blockconst int32_t bundle_id = curFrame()->bundleId();//获取当前帧的IDBackendId pose_id = createNFrameId(bundle_id);//创建标识位姿的IDsize_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 landmarksif (visual_initialized_ && curFrame()->isKeyframe()) {//如果视觉初始化已经完成，并且当前帧是关键帧curFrame()->set_T_w_imu(getPoseEstimate(states_[index]));//设置当前帧的位姿feature_handler_->initializeLandmarks(curFrame());//初始化路标点}// Add Landmark parameters and minimal resiudals at keyframeif (curFrame()->isKeyframe()) {addLandmarkParameterBlocksWithResiduals(curFrame());//添加路标点参数块及残差}// Add landmark observationsaddReprojectionErrorResidualBlocks(states_[index], curFrame());//添加重投影误差残差块return true;
}
addLandmarkParameterBlocksWithResiduals()
// Add landmark blocks to graph
void VisualEstimatorBase::addLandmarkParameterBlocksWithResiduals(const FramePtr& frame)
{// only add new landmarks at keyframeif (!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 landmarkif (point == nullptr || !isSeed(frame->type_vec_[kp_idx])) {//路标点为空或没有被初始化过continue;}// add new landmarks and corresponding observations at keyframeif (!isLandmarkInEstimator(createLandmarkId(point->id()))) {//路标点没有被添加到图优化中// check if the first frame is current keyframeif (point->obs_.front().frame_id == frame->id()) {//当前帧是路标点第一个观测帧，跳过continue;}CHECK(!std::isnan(point->pos_[0])) << "Point is nan!";// add the landmarkBackendId landmark_backend_id = createLandmarkId(point->id());//创建路标点的后端IDstd::shared_ptr<HomogeneousPointParameterBlock>point_parameter_block =//创建路标点参数块std::make_shared<HomogeneousPointParameterBlock>(point->pos(), landmark_backend_id.asInteger());//添加位置参数，idCHECK(graph_->addParameterBlock(point_parameter_block, Graph::HomogeneousPoint));// add landmark to mapauto 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 aheadif (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 landmarkif (added_cnt < 2) {//如果成功添加的观测数量不足2个，说明该路标点无效graph_->removeParameterBlock(landmark_backend_id.asInteger());//从图优化的参数块中移除该路标点的参数块point->in_ba_graph_ = false;//标记路标点已经从图中移除landmarks_map_.erase(it_landmark);//从图中移除该路标点}}}// Store keyframesactive_keyframes_.push_back(frame); //将当前帧添加到活动关键帧中
}
addReprojectionErrorResidualBlocks()
// 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());//创建帧的位姿后端IDCHECK(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(//创建路标点的后端IDframe->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 termstd::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;
}
二、RtkImuCameraRrrEstimator::estimate()
在336行
// Solve current graph
bool RtkImuCameraRrrEstimator::estimate()
{status_ = EstimatorStatus::Converged;// Optimize图优化optimize();// Get current sensor typeIdType new_state_type = states_[latest_state_index_].id.type();//获取当前状态类型// GNSS processesdouble 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 observationsif (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 processesif (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();
1、EstimatorBase::optimize()
// 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;
#endifif (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 solvergraph_->solve();//调用求解器进行优化// update covariance if (base_options_.compute_covariance && can_compute_covariance_) {updateCovariance(latestState());//更新协方差}
}
Solve()
在graph.h文件中找到如下：
// these are public for convenient manipulation/// \brief Ceres optionsceres::Solver::Options options;/// \brief Ceres optimization summaryceres::Solver::Summary summary;/// @brief Solve the optimization problem.
void solve(){Solve(options, problem_.get(), &summary);}
可以看到调用了Ceres库的Solve()函数进行解算，这里可以参考这篇优秀文章对Solve()进行学习：
【SLAM】Ceres优化库超详细解析_ceres库-CSDN博客
2、ambiguity_resolution_->solveRtk()
在gnss_estimator_base.h/686行找到：
// Ambiguity resolutionstd::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 storageambiguities_.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 parametersconst 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();//获取相位IDif (!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转换为整数作为参数块的IDuint64_t id = ambiguity_ids[i].asInteger();Spec ambiguity;ambiguity.id = ambiguity_ids[i];ambiguity.prn = prn;CHECK(graph_->parameterBlockExists(id));//检查相应的参数块是否存在于图中// parameter and residual blockambiguity.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;}}// wavelengthconst 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 selectionambiguity.elevation = elevation;//存储卫星的仰角curAmbs().push_back(ambiguity);//将当前计算的模糊度信息添加到模糊度集合中ambiguity_index_map.insert(std::make_pair(curAmbs().size() - 1, i));}//将当前计算的模糊度在模糊度集合中的索引与原始模糊度ID的索引之间建立映射关系// Get covariance of ambiguityCHECK(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 blocksGraph::ParameterBlockCollection parameters = graph_->parameters();//获取图优化中所有参数块的集合for (size_t p = 0; p < parameters.size(); p++) {//遍历所有参数块Parameter parameter;parameter.id = BackendId(parameters[p].first);//存储参数块的IDparameter.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 combinationformSatellitePairRtk();//应用卫星间差分和通道组合// 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 residualdouble 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 statusif (num_success_nl > 0) return Result::NlFix;//如果成功固定了窄巷通道，则返回 Result::NlFixelse if (num_success_wl > 0) return Result::WlFix;//如果成功固定了宽巷通道，则返回 Result::WlFixreturn Result::NoFix;
}
3、marginalization（）
根据传参类型边缘化
// 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;
}
frameMarginalization（）
// 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 marginalizationif (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 keyframeif (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 stateit = states_.erase(it);//删除it指向的状态，并返回指向被删除状态之后的迭代器n_state++;//增加状态计数器if (reached_first_keyframe) break;//如果已经到达第一个关键帧，就跳出循环}// landmarkseraseEmptyLandmarks();//删除空的路标addLandmarkParameterMarginBlocksWithResiduals(margin_keyframe_state);//添加路标参数边缘化项与残差相关// Apply marginalization and add the item into graphbool ret = applyMarginalization();//应用边缘化，并添加到图return ret;}return true;
}
applyMarginalization()
// 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){//如果边缘化误差对象中没有残差，则将其重置为nullptrmarginalization_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;
}
gnssMarginalization()
// Marginalization when the new state is a GNSS state
bool RtkImuCameraRrrEstimator::gnssMarginalization()
{// Check if we need marginalizationif (isFirstEpoch()) return true;//如果这是第一个历元，则不需要边缘化// Visual not initialzed, only handle GNSS and INSif (!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 residualsaddImuStateMarginBlockWithResiduals(oldestState());//为最老的IMU状态添加边缘化项和残差// ambiguityaddAmbiguityMarginBlocksWithResiduals(oldestAmbiguityState());//为最老的模糊度状态添加边缘化项和残差// frequencyaddFrequencyMarginBlocksWithResiduals(oldestState());//为最老的状态添加与频率相关的边缘化项和残差// Apply marginalization and add the item into graphbool ret = applyMarginalization();return ret;}// Visual initialized, handle all the sensorselse {//视觉初始化，处理所有传感器// Sparsify GNSS statessparsifyGnssStates();//对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 keyframeif (state.is_keyframe) break;//如果当前状态是关键帧，则跳出循环// check if GNSS stateif (state.id.type() != IdType::gPose) continue;//如果当前状态不是GNSS状态，则跳过// Erase old marginalization item删除旧的边缘化项if (!eraseOldMarginalization()) return false;// marginauto 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 measurementsif (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 stateit = states_.erase(it);//删除it指向的状态，并返回指向被删除状态之后的迭代器// just handle one in one timebool ret = applyMarginalization();//应用边缘化，并添加到图return ret;}}return true;
}
sparsifyGnssStates()
// Sparsify GNSS states to bound computational load对GNSS状态进行稀疏化处理，以限制计算负载
void RtkImuCameraRrrEstimator::sparsifyGnssStates()
{// Check if we need to sparsifystd::vector<BackendId> gnss_ids;//存储GNSS状态的IDstd::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 neighborsnum_neighbors.push_back(0);//初始化当前状态相邻状态的数量为0int 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状态的IDfor (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状态的IDif (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、文心一言
博主：
GICI-LIB源码阅读（二）主要的类和程序执行流程-CSDN博客
这篇关于GICI-rtk/imu/camera紧组合代码学习的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
