GVINS是基于VINS-MONO写的，视觉、IMU部分与VINS-MONO类似，可参考我的前一篇文章VINS-MONO学习
这篇文章主要解析与GNSS有关的部分。
持续更新中
estimator_node.cpp
main()
ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay()); // 订阅IMU数据
ros::Subscriber sub_feature = n.subscribe("/gvins_feature_tracker/feature", 2000, feature_callback); // 订阅图像特征点
ros::Subscriber sub_restart = n.subscribe("/gvins_feature_tracker/restart", 2000, restart_callback); // 订阅重启
ros::Subscriber sub_ephem, sub_glo_ephem, sub_gnss_meas, sub_gnss_iono_params;
ros::Subscriber sub_gnss_time_pluse_info, sub_local_trigger_info;
if (GNSS_ENABLE)
{
sub_ephem = n.subscribe(GNSS_EPHEM_TOPIC, 100, gnss_ephem_callback); // 订阅星历
sub_glo_ephem = n.subscribe(GNSS_GLO_EPHEM_TOPIC, 100, gnss_glo_ephem_callback); // 订阅GLONASS星历
sub_gnss_meas = n.subscribe(GNSS_MEAS_TOPIC, 100, gnss_meas_callback); // 订阅GNSS观测
sub_gnss_iono_params = n.subscribe(GNSS_IONO_PARAMS_TOPIC, 100, gnss_iono_params_callback); // 订阅电离层参数（Klobuchar模型）
if (GNSS_LOCAL_ONLINE_SYNC) // GNSS与本地在线时间同步
{
sub_gnss_time_pluse_info = n.subscribe(GNSS_TP_INFO_TOPIC, 100, gnss_tp_info_callback); // 订阅GNSS脉冲信息
sub_local_trigger_info = n.subscribe(LOCAL_TRIGGER_INFO_TOPIC, 100, local_trigger_info_callback); // 订阅相机触发信息
}
else
{
time_diff_gnss_local = GNSS_LOCAL_TIME_DIFF; // 截至23年3月 GPST-UTC=18s
estimator_ptr->inputGNSSTimeDiff(time_diff_gnss_local);
time_diff_valid = true;
}
}
}
gnss_tp_info_callback()
/* UBX-18010854-R08 P157 GNSS脉冲信息 */
void gnss_tp_info_callback(const GnssTimePulseInfoMsgConstPtr &tp_msg)
{
gtime_t tp_time = gpst2time(tp_msg->time.week, tp_msg->time.tow); // 转为计算机时间
if (tp_msg->utc_based || tp_msg->time_sys == SYS_GLO) // UTC->GPST tp_msg->utc_based 1:UTC 0:GNSS
tp_time = utc2gpst(tp_time);
else if (tp_msg->time_sys == SYS_GAL)
tp_time = gst2time(tp_msg->time.week, tp_msg->time.tow);
else if (tp_msg->time_sys == SYS_BDS)
tp_time = bdt2time(tp_msg->time.week, tp_msg->time.tow);
else if (tp_msg->time_sys == SYS_NONE)
{
std::cerr << "Unknown time system in GNSSTimePulseInfoMsg.\n";
return;
}
double gnss_ts = time2sec(tp_time);
std::lock_guard<std::mutex> lg(m_time);
next_pulse_time = gnss_ts; // 下一次脉冲时间
next_pulse_time_valid = true;
}
local_trigger_info_callback()
/* 在线时间同步--------------------------------------------------------------------------------------------------
* 用rqt_bag查看了一下GVINS配套的体育场数据包，相机触发事件5s一次，GNSS脉冲信息连续5s给的信息是一致的，
* GNSS脉冲信息给的是下一次脉冲的时间next_pulse_time（可准确转换为GPST），减去相机触发的本地时间戳，就是time_diff_gnss_local
*-------------------------------------------------------------------------------------------------------------*/
void local_trigger_info_callback(const gvins::LocalSensorExternalTriggerConstPtr &trigger_msg)
{
std::lock_guard<std::mutex> lg(m_time);
if (next_pulse_time_valid)
{
time_diff_gnss_local = next_pulse_time - trigger_msg->header.stamp.toSec();
estimator_ptr->inputGNSSTimeDiff(time_diff_gnss_local);
if (!time_diff_valid) // just get calibrated
std::cout << "time difference between GNSS and VI-Sensor got calibrated: "
<< std::setprecision(15) << time_diff_gnss_local << " s\n";
time_diff_valid = true;
}
}
feature_callback()
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
++ feature_msg_counter;
// 在时间同步后的前两帧决定skip_parameter
if (skip_parameter < 0 && time_diff_valid)
{
const double this_feature_ts = feature_msg->header.stamp.toSec()+time_diff_gnss_local;
if (latest_gnss_time > 0 && tmp_last_feature_time > 0)
{
if (abs(this_feature_ts - latest_gnss_time) > abs(tmp_last_feature_time - latest_gnss_time))
skip_parameter = feature_msg_counter%2; // skip this frame and afterwards
else
skip_parameter = 1 - (feature_msg_counter%2); // skip next frame and afterwards
}
cerr << "feature counter is " << feature_msg_counter << ", skip parameter is " << int(skip_parameter) << endl;
tmp_last_feature_time = this_feature_ts;
}
// feature_msg是按照图像原始数据频率发布 猜测：1：控制处理频率（20hz->10hz） 2：得到的图像时间戳更贴近GNSS观测时间戳
// 查看数据包后 GNSS 10hz 这样控制频率后 图像也是10hz左右
if (skip_parameter >= 0 && int(feature_msg_counter%2) != skip_parameter)
{
m_buf.lock();
feature_buf.push(feature_msg);
m_buf.unlock();
con.notify_one();
}
}
getMeasurements()
if (GNSS_ENABLE)
{
front_feature_ts += time_diff_gnss_local; // 转到GNSS时间戳
double front_gnss_ts = time2sec(gnss_meas_buf.front()[0]->time);
while (!gnss_meas_buf.empty() && front_gnss_ts < front_feature_ts-MAX_GNSS_CAMERA_DELAY) // 删除旧的gnss数据
{
ROS_WARN("throw gnss, only should happen at the beginning");
gnss_meas_buf.pop();
if (gnss_meas_buf.empty()) return false;
front_gnss_ts = time2sec(gnss_meas_buf.front()[0]->time);
}
if (gnss_meas_buf.empty())
{
ROS_WARN("wait for gnss...");
return false;
}
else if (abs(front_gnss_ts-front_feature_ts) < MAX_GNSS_CAMERA_DELAY) // 添加
{
gnss_msg = gnss_meas_buf.front();
gnss_meas_buf.pop();
}
}
estimator.cpp
processGNSS()
void Estimator::processGNSS(const std::vector<ObsPtr> &gnss_meas)
{
std::vector<ObsPtr> valid_meas;
std::vector<EphemBasePtr> valid_ephems;
for (auto obs : gnss_meas)
{
// filter according to system
uint32_t sys = satsys(obs->sat, NULL);
if (sys != SYS_GPS && sys != SYS_GLO && sys != SYS_GAL && sys != SYS_BDS)
continue;
// if not got cooresponding ephemeris yet
if (sat2ephem.count(obs->sat) == 0)
continue;
if (obs->freqs.empty()) continue; // no valid signal measurement
int freq_idx = -1;
L1_freq(obs, &freq_idx);
if (freq_idx < 0) continue; // no L1 observation
double obs_time = time2sec(obs->time); // GPST
std::map<double, size_t> time2index = sat2time_index.at(obs->sat); // <toe(广播星历参考时间), 星历索引>
double ephem_time = EPH_VALID_SECONDS;
size_t ephem_index = -1;
for (auto ti : time2index) // 遍历该卫星的所有星历
{
if (std::abs(ti.first - obs_time) < ephem_time) // 找距离本次观测时间最近的星历
{
ephem_time = std::abs(ti.first - obs_time);
ephem_index = ti.second;
}
}
if (ephem_time >= EPH_VALID_SECONDS) // obs_time超出星历参考时刻以外2h 星历无效
{
cerr << "ephemeris not valid anymore\n";
continue;
}
const EphemBasePtr &best_ephem = sat2ephem.at(obs->sat).at(ephem_index);
// filter by tracking status
LOG_IF(FATAL, freq_idx < 0) << "No L1 observation found.\n";
if (obs->psr_std[freq_idx] > GNSS_PSR_STD_THRES || // 观测质量不合格
obs->dopp_std[freq_idx] > GNSS_DOPP_STD_THRES)
{
sat_track_status[obs->sat] = 0;
continue;
}
else // 增加该星的跟踪次数
{
if (sat_track_status.count(obs->sat) == 0)
sat_track_status[obs->sat] = 0;
++ sat_track_status[obs->sat];
}
if (sat_track_status[obs->sat] < GNSS_TRACK_NUM_THRES)
continue; // not being tracked for enough epochs
// filter by elevation angle 高度角（仰角）
if (gnss_ready) // GNSS与VIO初始化后
{
Eigen::Vector3d sat_ecef;
if (sys == SYS_GLO) // 计算粗略卫星位置，计算方位角、高度角
sat_ecef = geph2pos(obs->time, std::dynamic_pointer_cast<GloEphem>(best_ephem), NULL);
else
sat_ecef = eph2pos(obs->time, std::dynamic_pointer_cast<Ephem>(best_ephem), NULL);
double azel[2] = {0, M_PI/2.0};
sat_azel(ecef_pos, sat_ecef, azel);
if (azel[1] < GNSS_ELEVATION_THRES*M_PI/180.0)
continue;
}
valid_meas.push_back(obs); // 存入
valid_ephems.push_back(best_ephem);
}
gnss_meas_buf[frame_count] = valid_meas;
gnss_ephem_buf[frame_count] = valid_ephems;
}
GNSSVIAlign()
bool Estimator::GNSSVIAlign()
{
if (solver_flag == INITIAL) // visual-inertial not initialized
return false;
if (gnss_ready) // GNSS-VI already initialized
return true;
for (uint32_t i = 0; i < (WINDOW_SIZE+1); ++i) // 检查窗口内GNSS数据
{
if (gnss_meas_buf[i].empty() || gnss_meas_buf[i].size() < 10)
return false;
}
// check horizontal velocity excitation
Eigen::Vector2d avg_hor_vel(0.0, 0.0);
for (uint32_t i = 0; i < (WINDOW_SIZE+1); ++i)
avg_hor_vel += Vs[i].head<2>().cwiseAbs();
avg_hor_vel /= (WINDOW_SIZE+1);
if (avg_hor_vel.norm() < 0.3)
{
std::cerr << "velocity excitation not enough for GNSS-VI alignment.\n";
return false;
}
std::vector<std::vector<ObsPtr>> curr_gnss_meas_buf;
std::vector<std::vector<EphemBasePtr>> curr_gnss_ephem_buf;
for (uint32_t i = 0; i < (WINDOW_SIZE+1); ++i)
{
curr_gnss_meas_buf.push_back(gnss_meas_buf[i]);
curr_gnss_ephem_buf.push_back(gnss_ephem_buf[i]);
}
GNSSVIInitializer gnss_vi_initializer(curr_gnss_meas_buf, curr_gnss_ephem_buf, latest_gnss_iono_params);
// 1. get a rough global location 获得接收机在ECEF系下粗略坐标、接收机钟差
Eigen::Matrix<double, 7, 1> rough_xyzt;
rough_xyzt.setZero();
if (!gnss_vi_initializer.coarse_localization(rough_xyzt))
{
std::cerr << "Fail to obtain a coarse location.\n";
return false;
}
// 2. perform yaw alignment w系和当地ENU系对齐 w系的z轴和ENU的U平行，绕z轴转动对齐（航向）
std::vector<Eigen::Vector3d> local_vs;
for (uint32_t i = 0; i < (WINDOW_SIZE+1); ++i)
local_vs.push_back(Vs[i]); // b系在w系下的速度
Eigen::Vector3d rough_anchor_ecef = rough_xyzt.head<3>(); // 接收机在ECEF下的粗糙位置
double aligned_yaw = 0;
double aligned_rcv_ddt = 0;
if (!gnss_vi_initializer.yaw_alignment(local_vs, rough_anchor_ecef, aligned_yaw, aligned_rcv_ddt))
{
std::cerr << "Fail to align ENU and local frames.\n";
return false;
}
// std::cout << "aligned_yaw is " << aligned_yaw*180.0/M_PI << '\n';
// 3. perform anchor refinement 锚点重定义
std::vector<Eigen::Vector3d> local_ps;
for (uint32_t i = 0; i < (WINDOW_SIZE+1); ++i)
local_ps.push_back(Ps[i]); // b系在w系下的位置
Eigen::Matrix<double, 7, 1> refined_xyzt;
refined_xyzt.setZero();
if (!gnss_vi_initializer.anchor_refinement(local_ps, aligned_yaw,
aligned_rcv_ddt, rough_xyzt, refined_xyzt))
{
std::cerr << "Fail to refine anchor point.\n";
return false;
}
// std::cout << "refined anchor point is " << std::setprecision(20)
// << refined_xyzt.head<3>().transpose() << '\n';
// restore GNSS states
uint32_t one_observed_sys = static_cast<uint32_t>(-1);
for (uint32_t k = 0; k < 4; ++k)
{
if (rough_xyzt(k+3) != 0)
{
one_observed_sys = k;
break;
}
}
for (uint32_t i = 0; i < (WINDOW_SIZE+1); ++i)
{
para_rcv_ddt[i] = aligned_rcv_ddt;
for (uint32_t k = 0; k < 4; ++k)
{
if (rough_xyzt(k+3) == 0)
para_rcv_dt[i*4+k] = refined_xyzt(3+one_observed_sys) + aligned_rcv_ddt * i;
else
para_rcv_dt[i*4+k] = refined_xyzt(3+k) + aligned_rcv_ddt * i;
}
}
anc_ecef = refined_xyzt.head<3>();
R_ecef_enu = ecef2rotation(anc_ecef);
yaw_enu_local = aligned_yaw;
return true;
}
GNSS与VIO初始化参考
1、参考1：含中文解析代码
2、参考2
yaw_alignment()
/* w系与ENU系对齐 */
bool GNSSVIInitializer::yaw_alignment(const std::vector<Eigen::Vector3d> &local_vs,
const Eigen::Vector3d &rough_anchor_ecef, double &aligned_yaw, double &rcv_ddt)
{
aligned_yaw = 0;
rcv_ddt = 0;
double est_yaw = 0;
double est_rcv_ddt = 0;
Eigen::Matrix3d rough_R_ecef_enu = ecef2rotation(rough_anchor_ecef); // ENU到ECEF的旋转矩阵
uint32_t align_iter = 0;
double align_dx_norm = 1.0;
while (align_iter < MAX_ITERATION && align_dx_norm > CONVERGENCE_EPSILON) // 迭代最小二乘求解接收机时钟钟差变化率和偏航角
{
Eigen::MatrixXd align_G(num_all_meas, 2); // （观测（残差）方程数量、参数数量）
align_G.setZero();
align_G.col(1).setOnes();
Eigen::VectorXd align_b(num_all_meas);
align_b.setZero();
Eigen::Matrix3d align_R_enu_local(Eigen::AngleAxisd(est_yaw, Eigen::Vector3d::UnitZ())); // enu系<-w系初始值：绕Z轴转est_yaw的轴角 转 旋转矩阵
Eigen::Matrix3d align_tmp_M; // 绕Z轴旋转est_yaw角度的旋转矩阵 求导
align_tmp_M << -sin(est_yaw), -cos(est_yaw), 0,
cos(est_yaw), -sin(est_yaw), 0,
0 , 0 , 0;
// 构建多普勒测量的残差和雅可比矩阵
uint32_t align_counter = 0;
for (uint32_t i = 0; i < gnss_meas_buf.size(); ++i)
{
Eigen::Matrix<double, 4, 1> ecef_vel_ddt;
ecef_vel_ddt.head<3>() = rough_R_ecef_enu * align_R_enu_local * local_vs[i]; // 得到接收机在ECEF系下的速度
ecef_vel_ddt(3) = est_rcv_ddt; // 接收机时钟漂移率
Eigen::VectorXd epoch_res; // 多普勒测量残差
Eigen::MatrixXd epoch_J; // 多普勒测量雅可比
dopp_res(ecef_vel_ddt, rough_anchor_ecef, gnss_meas_buf[i], all_sat_states[i], epoch_res, epoch_J);
align_b.segment(align_counter, gnss_meas_buf[i].size()) = epoch_res;
/* dopp_res中的雅可比矩阵J是对速度求导数，优化变量实际为航向角，G矩阵要乘以速度对yaw的导数
* align_tmp_M = ENU<-LOCAL WORLD d()/d(yaw)
* align_tmp_M*local_vs[i] = ENU系下 d(v)/d(yaw)
* rough_R_ecef_enu*align_tmp_M*local_vs[i] = ECEF系下 d(v)/d(yaw) */
align_G.block(align_counter, 0, gnss_meas_buf[i].size(), 1) =
epoch_J.leftCols(3)*rough_R_ecef_enu*align_tmp_M*local_vs[i];
align_counter += gnss_meas_buf[i].size();
}
Eigen::VectorXd dx = -(align_G.transpose()*align_G).inverse() * align_G.transpose() * align_b;
est_yaw += dx(0);
est_rcv_ddt += dx(1);
align_dx_norm = dx.norm();
++ align_iter;
}
if (align_iter > MAX_ITERATION)
{
std::cerr << "Fail to initialize yaw offset.\n";
return false;
}
// 调整航向角到[-180, 180]
aligned_yaw = est_yaw;
if (aligned_yaw > M_PI)
aligned_yaw -= floor(est_yaw/(2.0*M_PI) + 0.5) * (2.0*M_PI);
else if (aligned_yaw < -M_PI)
aligned_yaw -= ceil(est_yaw/(2.0*M_PI) - 0.5) * (2.0*M_PI);
rcv_ddt = est_rcv_ddt;
return true;
}
anchor_refinement()
bool GNSSVIInitializer::anchor_refinement(const std::vector<Eigen::Vector3d> &local_ps,
const double aligned_yaw, const double aligned_ddt,
const Eigen::Matrix<double, 7, 1> &rough_ecef_dt, Eigen::Matrix<double, 7, 1> &refined_ecef_dt)
{
refined_ecef_dt.setZero();
Eigen::Matrix3d aligned_R_enu_local(Eigen::AngleAxisd(aligned_yaw, Eigen::Vector3d::UnitZ())); // w系到enu系的转换矩阵；
// refine anchor point and receiver clock bias
Eigen::Vector3d refine_anchor = rough_ecef_dt.head<3>();
Eigen::Vector4d refine_dt = rough_ecef_dt.tail<4>();
uint32_t refine_iter = 0;
double refine_dx_norm = 1.0;
std::vector<uint32_t> unobserved_sys;
for (uint32_t k = 0; k < 4; ++k)
{
if (rough_ecef_dt(3+k) == 0)
unobserved_sys.push_back(k);
}
while (refine_iter < MAX_ITERATION && refine_dx_norm > CONVERGENCE_EPSILON)
{
Eigen::MatrixXd refine_G(num_all_meas+unobserved_sys.size(), 7);
Eigen::VectorXd refine_b(num_all_meas+unobserved_sys.size());
refine_G.setZero();
refine_b.setZero();
uint32_t refine_counter = 0;
Eigen::Matrix3d refine_R_ecef_enu = ecef2rotation(refine_anchor); // enu系到ecef系的旋转矩阵
Eigen::Matrix3d refine_R_ecef_local = refine_R_ecef_enu * aligned_R_enu_local; // w系到ecef系的旋转矩阵
for (uint32_t i = 0; i < gnss_meas_buf.size(); ++i)
{
Eigen::Matrix<double, 7, 1> ecef_xyz_dt;
ecef_xyz_dt.head<3>() = refine_R_ecef_local * local_ps[i] + refine_anchor; // 锚点与w系原点重合，计算结果应该是每帧b系在ECEF系的位置？
ecef_xyz_dt.tail<4>() = refine_dt + aligned_ddt * i * Eigen::Vector4d::Ones(); // 钟差
Eigen::VectorXd epoch_res;
Eigen::MatrixXd epoch_J;
std::vector<Eigen::Vector2d> tmp_atmos_delay, tmp_sv_azel;
psr_res(ecef_xyz_dt, gnss_meas_buf[i], all_sat_states[i], iono_params,
epoch_res, epoch_J, tmp_atmos_delay, tmp_sv_azel);
refine_b.segment(refine_counter, gnss_meas_buf[i].size()) = epoch_res;
refine_G.middleRows(refine_counter, gnss_meas_buf[i].size()) = epoch_J;
refine_counter += gnss_meas_buf[i].size();
}
for (uint32_t k : unobserved_sys)
{
refine_b(refine_counter) = 0;
refine_G(refine_counter, k+3) = 1.0;
++ refine_counter;
}
Eigen::VectorXd dx = -(refine_G.transpose()*refine_G).inverse() * refine_G.transpose() * refine_b;
refine_anchor += dx.head<3>();
refine_dt += dx.tail<4>();
refine_dx_norm = dx.norm();
++ refine_iter;
}
if (refine_iter > MAX_ITERATION)
{
std::cerr << "Fail to perform anchor refinement.\n";
return false;
}
refined_ecef_dt.head<3>() = refine_anchor;
refined_ecef_dt.tail<4>() = refine_dt;
return true;
}
updateGNSSStatistics()
// GNSS初始化完成后，更新参数
void Estimator::updateGNSSStatistics()
{
R_enu_local = Eigen::AngleAxisd(yaw_enu_local, Eigen::Vector3d::UnitZ()); // w系到enu系的旋转矩阵
enu_pos = R_enu_local * Ps[WINDOW_SIZE]; // 令enu为参考系
enu_vel = R_enu_local * Vs[WINDOW_SIZE];
enu_ypr = Utility::R2ypr(R_enu_local*Rs[WINDOW_SIZE]); // 欧拉角组（deg）
ecef_pos = anc_ecef + R_ecef_enu * enu_pos; // b系在ECEF下的位置
}
}
optimization()
A. 优化
...
// gnss相关参数块 显式定义
if (gnss_ready)
{
problem.AddParameterBlock(para_yaw_enu_local, 1); // w系到enu系的航向角
Eigen::Vector2d avg_hor_vel(0.0, 0.0);
for (uint32_t i = 0; i <= WINDOW_SIZE; ++i)
avg_hor_vel += Vs[i].head<2>().cwiseAbs();
avg_hor_vel /= (WINDOW_SIZE+1);
// cerr << "avg_hor_vel is " << avg_vel << endl;
if (avg_hor_vel.norm() < 0.3) // 速度不够，不优化航向角
{
// std::cerr << "velocity excitation not enough, fix yaw angle.\n";
problem.SetParameterBlockConstant(para_yaw_enu_local);
}
for (uint32_t i = 0; i <= WINDOW_SIZE; ++i)
{
if (gnss_meas_buf[i].size() < 10) // GNSS观测数据不够，不优化航向角
problem.SetParameterBlockConstant(para_yaw_enu_local);
}
problem.AddParameterBlock(para_anc_ecef, 3); // 锚点（w系与ENU系的共同原点）
// problem.SetParameterBlockConstant(para_anc_ecef);
for (uint32_t i = 0; i <= WINDOW_SIZE; ++i)
{
for (uint32_t k = 0; k < 4; ++k)
problem.AddParameterBlock(para_rcv_dt+i*4+k, 1); // 钟差
problem.AddParameterBlock(para_rcv_ddt+i, 1); // 钟漂
}
}
TicToc t_whole, t_prepare;
vector2double();
// 猜测：在优化中固定初始化后的第一帧b系在w系下的位姿（由于绝对X、Y、Z、YAW不可观）
if (first_optimization)
{
std::vector<double> anchor_value;
for (uint32_t k = 0; k < 7; ++k)
anchor_value.push_back(para_Pose[0][k]);
PoseAnchorFactor *pose_anchor_factor = new PoseAnchorFactor(anchor_value);
problem.AddResidualBlock(pose_anchor_factor, NULL, para_Pose[0]);
first_optimization = false;
}
...
// gnss相关残差和雅可比
if (gnss_ready)
{
for(int i = 0; i <= WINDOW_SIZE; ++i)
{
// cerr << "size of gnss_meas_buf[" << i << "] is " << gnss_meas_buf[i].size() << endl;
const std::vector<ObsPtr> &curr_obs = gnss_meas_buf[i]; // 观测数据
const std::vector<EphemBasePtr> &curr_ephem = gnss_ephem_buf[i]; // 星历
for (uint32_t j = 0; j < curr_obs.size(); ++j)
{
const uint32_t sys = satsys(curr_obs[j]->sat, NULL); // 系统
const uint32_t sys_idx = gnss_comm::sys2idx.at(sys); // 索引
int lower_idx = -1;
const double obs_local_ts = time2sec(curr_obs[j]->time) - diff_t_gnss_local; // 图像时间戳
if (Headers[i].stamp.toSec() > obs_local_ts) // 这帧图像时间戳比gnss大
lower_idx = (i==0? 0 : i-1);
else // 这帧图像时间戳比gnss小
lower_idx = (i==WINDOW_SIZE? WINDOW_SIZE-1 : i);
const double lower_ts = Headers[lower_idx].stamp.toSec(); // 一般：lower_ts < obs_local_ts < upper_ts
const double upper_ts = Headers[lower_idx+1].stamp.toSec(); // 特殊：obs_local_ts < lower_ts < upper_ts < obs_local_ts
const double ts_ratio = (upper_ts-obs_local_ts) / (upper_ts-lower_ts);
// 构造代价函数 计算残差和雅可比
GnssPsrDoppFactor *gnss_factor = new GnssPsrDoppFactor(curr_obs[j],
curr_ephem[j], latest_gnss_iono_params, ts_ratio);
problem.AddResidualBlock(gnss_factor, NULL, para_Pose[lower_idx],
para_SpeedBias[lower_idx], para_Pose[lower_idx+1], para_SpeedBias[lower_idx+1],
para_rcv_dt+i*4+sys_idx, para_rcv_ddt+i, para_yaw_enu_local, para_anc_ecef);
}
}
// build relationship between rcv_dt and rcv_ddt
for (size_t k = 0; k < 4; ++k)
{
for (uint32_t i = 0; i < WINDOW_SIZE; ++i)
{
const double gnss_dt = Headers[i+1].stamp.toSec() - Headers[i].stamp.toSec();
DtDdtFactor *dt_ddt_factor = new DtDdtFactor(gnss_dt);
problem.AddResidualBlock(dt_ddt_factor, NULL, para_rcv_dt+i*4+k,
para_rcv_dt+(i+1)*4+k, para_rcv_ddt+i, para_rcv_ddt+i+1);
}
}
// add rcv_ddt smooth factor
for (int i = 0; i < WINDOW_SIZE; ++i)
{
DdtSmoothFactor *ddt_smooth_factor = new DdtSmoothFactor(GNSS_DDT_WEIGHT);
problem.AddResidualBlock(ddt_smooth_factor, NULL, para_rcv_ddt+i, para_rcv_ddt+i+1);
}
}
PoseAnchorFactor::Evaluate()
bool PoseAnchorFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
Eigen::Map<const Eigen::Matrix<double, 7, 1>> pose(parameters[0]);
Eigen::Map<Eigen::Matrix<double, 6, 1>> res(residuals);
res.head<3>() = pose.head<3>() - _anchor_point.head<3>(); // 位置残差
const Eigen::Quaterniond curr_q(pose.tail<4>());
const Eigen::Quaterniond anchor_q(_anchor_point.tail<4>());
res.tail<3>() = 2.0 * (curr_q*anchor_q.inverse()).vec(); // 旋转残差
res *= sqrt_info; // 大权重
if (jacobians && jacobians[0])
{
Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor>> J(jacobians[0]);
J.setZero();
J.topLeftCorner<3, 3>().setIdentity(); // res = (x_p-anchor_p)*sqrt_info
Eigen::Quaterniond anchor_q_inv = anchor_q.inverse();
Eigen::Matrix3d J_q; // res = 2*(q_x*q_anchor_q_inv)xyz*sqrt_info
J_q << anchor_q_inv.w(), anchor_q_inv.z(), -anchor_q_inv.y(),
-anchor_q_inv.z(), anchor_q_inv.w(), anchor_q_inv.x(),
anchor_q_inv.y(), -anchor_q_inv.x(), anchor_q_inv.w();
J.block<3, 3>(3, 3) = J_q;
J *= 2.0*sqrt_info; // 为什么平移部分也要乘2 ????
}
return true;
}
思考：这一段中雅可比J全部乘2不是很理解，知道的朋友求解惑!
GnssPsrDoppFactor::Evaluate()
GnssPsrDoppFactor 相关参数
para[0]: position and orientation at time k
para[1]: velocity and acc/gyro bias at time k
para[2]: position and orientation at time k+1
para[3]: velocity and acc/gyro bias at time k+1
para[4]: receiver clock bias in light travelling distance (m)
para[5]: receiver clock bias change rate in clock bias light travelling distance per second (m/s)
para[6]: yaw difference between ENU and local coordinate (rad)
para[7]: anchor point’s ECEF coordinate
bool GnssPsrDoppFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);
Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
double rcv_dt = parameters[4][0];
double rcv_ddt = parameters[5][0];
double yaw_diff = parameters[6][0];
Eigen::Vector3d ref_ecef(parameters[7][0], parameters[7][1], parameters[7][2]);
const Eigen::Vector3d local_pos = ratio*Pi + (1.0-ratio)*Pj; // 通过相邻两帧图像的PV线性插值出对齐GNSS数据时刻的PV
const Eigen::Vector3d local_vel = ratio*Vi + (1.0-ratio)*Vj;
double sin_yaw_diff = std::sin(yaw_diff);
double cos_yaw_diff = std::cos(yaw_diff);
Eigen::Matrix3d R_enu_local;
R_enu_local << cos_yaw_diff, -sin_yaw_diff, 0,
sin_yaw_diff, cos_yaw_diff, 0,
0 , 0 , 1;
Eigen::Matrix3d R_ecef_enu = ecef2rotation(ref_ecef);
Eigen::Matrix3d R_ecef_local = R_ecef_enu * R_enu_local;
Eigen::Vector3d P_ecef = R_ecef_local * local_pos + ref_ecef;
Eigen::Vector3d V_ecef = R_ecef_local * local_vel;
double ion_delay = 0, tro_delay = 0;
double azel[2] = {0, M_PI/2.0};
if (P_ecef.norm() > 0)
{
sat_azel(P_ecef, sv_pos, azel); // 方位角、高度角
Eigen::Vector3d rcv_lla = ecef2geo(P_ecef);
tro_delay = calculate_trop_delay(obs->time, rcv_lla, azel); // 对流层延迟
ion_delay = calculate_ion_delay(obs->time, iono_paras, rcv_lla, azel); // 电离层延迟
}
double sin_el = sin(azel[1]);
double sin_el_2 = sin_el*sin_el;
double pr_weight = sin_el_2 / pr_uura * relative_sqrt_info; // 伪距权重
double dp_weight = sin_el_2 / dp_uura * relative_sqrt_info * PSR_TO_DOPP_RATIO; // 多普勒权重
Eigen::Vector3d rcv2sat_ecef = sv_pos - P_ecef;
Eigen::Vector3d rcv2sat_unit = rcv2sat_ecef.normalized();
const double psr_sagnac = EARTH_OMG_GPS*(sv_pos(0)*P_ecef(1)-sv_pos(1)*P_ecef(0))/LIGHT_SPEED; // 地球自转
double psr_estimated = rcv2sat_ecef.norm() + psr_sagnac + rcv_dt - svdt*LIGHT_SPEED +
ion_delay + tro_delay + tgd*LIGHT_SPEED;
residuals[0] = (psr_estimated - obs->psr[freq_idx]) * pr_weight; // 伪距残差
const double dopp_sagnac = EARTH_OMG_GPS/LIGHT_SPEED*(sv_vel(0)*P_ecef(1)+
sv_pos(0)*V_ecef(1) - sv_vel(1)*P_ecef(0) - sv_pos(1)*V_ecef(0));
double dopp_estimated = (sv_vel - V_ecef).dot(rcv2sat_unit) + dopp_sagnac + rcv_ddt - svddt*LIGHT_SPEED;
const double wavelength = LIGHT_SPEED / freq;
residuals[1] = (dopp_estimated + obs->dopp[freq_idx]*wavelength) * dp_weight; // 多普勒残差
if (jacobians) // 对各个优化量求取雅可比
{
// J_Pi i时刻位姿
if (jacobians[0])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> J_Pi(jacobians[0]);
J_Pi.setZero();
J_Pi.topLeftCorner<1, 3>() = -rcv2sat_unit.transpose() * R_ecef_local * pr_weight * ratio; // residuals[0]对Pi求一阶导
const double norm3 = pow(rcv2sat_ecef.norm(), 3); // 卫地距的3次方
const double norm2 = rcv2sat_ecef.squaredNorm(); // 卫地距的2次方
Eigen::Matrix3d unit2rcv_pos;
for (size_t i = 0; i < 3; ++i)
{
for (size_t j = 0; j < 3; ++j)
{
if (i == j)
unit2rcv_pos(i, j) = (norm2-rcv2sat_ecef(i)*rcv2sat_ecef(i))/norm3;
else
unit2rcv_pos(i, j) = (-rcv2sat_ecef(i)*rcv2sat_ecef(j))/norm3;
}
}
unit2rcv_pos *= -1;
J_Pi.bottomLeftCorner<1, 3>() = (sv_vel-V_ecef).transpose() * unit2rcv_pos *
R_ecef_local * dp_weight * ratio; // residuals[1]对Pi求一阶导
}
// J_Vi i时刻速度
if (jacobians[1])
{
Eigen::Map<Eigen::Matrix<double, 2, 9, Eigen::RowMajor>> J_Vi(jacobians[1]);
J_Vi.setZero();
J_Vi.bottomLeftCorner<1, 3>() = rcv2sat_unit.transpose() * (-1.0) *
R_ecef_local * dp_weight * ratio;
}
// J_Pj
if (jacobians[2])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> J_Pj(jacobians[2]);
J_Pj.setZero();
J_Pj.topLeftCorner<1, 3>() = -rcv2sat_unit.transpose() * R_ecef_local * pr_weight * (1.0-ratio);
const double norm3 = pow(rcv2sat_ecef.norm(), 3);
const double norm2 = rcv2sat_ecef.squaredNorm();
Eigen::Matrix3d unit2rcv_pos;
for (size_t i = 0; i < 3; ++i)
{
for (size_t j = 0; j < 3; ++j)
{
if (i == j)
unit2rcv_pos(i, j) = (norm2-rcv2sat_ecef(i)*rcv2sat_ecef(i))/norm3;
else
unit2rcv_pos(i, j) = (-rcv2sat_ecef(i)*rcv2sat_ecef(j))/norm3;
}
}
unit2rcv_pos *= -1;
J_Pj.bottomLeftCorner<1, 3>() = (sv_vel-V_ecef).transpose() * unit2rcv_pos *
R_ecef_local * dp_weight * (1.0-ratio);
}
// J_Vj
if (jacobians[3])
{
Eigen::Map<Eigen::Matrix<double, 2, 9, Eigen::RowMajor>> J_Vj(jacobians[3]);
J_Vj.setZero();
J_Vj.bottomLeftCorner<1, 3>() = rcv2sat_unit.transpose() * (-1.0) *
R_ecef_local * dp_weight * (1.0-ratio);
}
// J_rcv_dt
if (jacobians[4])
{
jacobians[4][0] = 1.0 * pr_weight;
jacobians[4][1] = 0;
}
// J_rcv_ddt
if (jacobians[5])
{
jacobians[5][0] = 0;
jacobians[5][1] = 1.0 * dp_weight;
}
// J_yaw_diff
if (jacobians[6])
{
Eigen::Matrix3d d_yaw;
// 绕Z轴旋转yaw 的导数
d_yaw << -sin_yaw_diff, -cos_yaw_diff, 0,
cos_yaw_diff, -sin_yaw_diff, 0,
0 , 0 , 0;
jacobians[6][0] = -rcv2sat_unit.dot(R_ecef_enu * d_yaw * local_pos) * pr_weight;
jacobians[6][1] = -rcv2sat_unit.dot(R_ecef_enu * d_yaw * local_vel) * dp_weight;
}
// J_ref_ecef, approximation for simplicity
if (jacobians[7])
{
Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> J_ref_ecef(jacobians[7]);
J_ref_ecef.setZero();
J_ref_ecef.row(0) = -rcv2sat_unit.transpose() * pr_weight;
}
}
return true;
}
多普勒残差对位置的雅可比推导
DtDdtFactor::Evaluate()
para[0]: rev_dt (t) in light travelling distance (m)
para[1]: rev_dt (t+1) in light travelling distance (m)
para[2]: rev_ddt(t) in light travelling distance per second (m/s)
para[3]: rev_ddt(t+1) in light travelling distance per second (m/s)
DdtSmoothFactor::Evaluate()
para[0]: rev_ddt (t) in light travelling distance (m)
para[1]: rev_ddt (t+1) in light travelling distance (m)
和钟差、钟漂相关的约束也不是很明白 论文中 钟漂建模为随机游走过程是怎么体现的
1302
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
