学习笔记
定位结算中伪距的地球自转改正
// The stride length of the dynamic_autodiff_cost_function evaluator.
static const int kStride = 10; // 动态自动差异成本函数评估器的步长 （ Ceres自动求导步长 ）
class gnssSinglePointPositioning
{
ros::NodeHandle nh;
// ros subscriber
// ros::Publisher发布节点 ros subscriber订阅节点
ros::Subscriber gnss_raw_sub; // 订阅GNSS原始数据
ros::Publisher pub_WLS, pub_FGO; // 发布 加权最小二乘 & 图优化
std::queue<nlosExclusion::GNSS_Raw_ArrayConstPtr> gnss_raw_buf; // 创建一个队列
std::map<double, nlosExclusion::GNSS_Raw_Array> gnss_raw_map;
std::mutex m_gnss_raw_mux; // 该类表示普通的互斥锁, 不能递归使用
std::mutex optimize_mux;
std::thread optimizationThread;
GNSS_Tools m_GNSS_Tools; // utilities 公用
nav_msgs::Path fgo_path; // 图优化轨迹集合????
int gnss_frame = 0;
Eigen::Matrix<double, 3,1> ENULlhRef; // 创建一个3×1 double型矩阵，装位置信息：经 纬 高
bool hasNewData =false;
public:
// from Weisong
struct pseudorangeFactor
{
pseudorangeFactor(std::string sat_sys, double s_g_x, double s_g_y, double s_g_z, double pseudorange, double var)
:sat_sys(sat_sys),s_g_x(s_g_x), s_g_y(s_g_y), s_g_z(s_g_z), pseudorange(pseudorange),var(var){}
template <typename T> // 定义模板的固定格式 T是模板实例化时才知道的类型
// template的使用是为了简化不同类型的函数和类的重复定义
bool operator()(const T* state, T* residuals) const
{
T est_pseudorange;
T delta_x = pow((state[0] - s_g_x),2); // pow函数 pow(a,b)表示a的b次方
T delta_y = pow((state[1] - s_g_y),2);
T delta_z = pow((state[2] - s_g_z),2);
est_pseudorange = sqrt(delta_x+ delta_y + delta_z); // 卫星和接收机之间的几何距离
double OMGE_ = 7.2921151467E-5; // 地球自转速率
double CLIGHT_ = 299792458.0; // 光速
est_pseudorange = est_pseudorange + OMGE_ * (s_g_x*state[1]-s_g_y*state[0])/CLIGHT_;
if(sat_sys == "GPS")
{
est_pseudorange = est_pseudorange - state[3];
} // GPS时钟偏置
else
{
est_pseudorange = est_pseudorange - state[4];
} // Beidou时钟偏置
residuals[0] = (est_pseudorange - T(pseudorange)) / T(var); // 定义残差
return true;
}
double s_g_x, s_g_y, s_g_z, pseudorange, var;
std::string sat_sys; // satellite system
};
// from Weisong
//伪距约束
struct pseudorangeConstraint
{
typedef ceres::DynamicAutoDiffCostFunction<pseudorangeConstraint, kStride>
// Ceres自动求导机制，设定步长10
PseudorangeCostFunction;
pseudorangeConstraint(std::string sat_sys, double s_g_x, double s_g_y, double s_g_z, double pseudorange, double var, int keyIndex)
:sat_sys(sat_sys),s_g_x(s_g_x), s_g_y(s_g_y), s_g_z(s_g_z), pseudorange(pseudorange),var(var), keyIndex(keyIndex){}
template <typename T>
bool operator()(T const* const* state, T* residuals) const
{
T est_pseudorange;
T delta_x = pow((state[keyIndex][0] - s_g_x),2);
T delta_y = pow((state[keyIndex][1] - s_g_y),2);
T delta_z = pow((state[keyIndex][2] - s_g_z),2);
est_pseudorange = sqrt(delta_x+ delta_y + delta_z);
double OMGE_ = 7.2921151467E-5; // 地球自传速率
double CLIGHT_ = 299792458.0; // 光速
est_pseudorange = est_pseudorange + OMGE_ * (s_g_x*state[keyIndex][1]-s_g_y*state[keyIndex][0])/CLIGHT_;
// 第二项为 卫星定位系统中的地球自转改正
if(sat_sys == "GPS")
{
est_pseudorange = est_pseudorange - state[keyIndex][3];
}
else
{
est_pseudorange = est_pseudorange - state[keyIndex][4];
}
residuals[0] = (est_pseudorange - T(pseudorange)) / T(var);
return true;
}
// 工厂化方法，从伪距约束中创建一个CostFunction，以方便添加到CERES问题中。方便地添加到ceres问题中。
// Factory method to create a CostFunction from a pseudorangeConstraint to
// conveniently add to a ceres problem.
static PseudorangeCostFunction* Create(std::string sat_sys, double s_g_x, double s_g_y, double s_g_z, double pseudorange, double var, int keyIndex) {
pseudorangeConstraint* constraint = new pseudorangeConstraint(
sat_sys, s_g_x, s_g_y, s_g_z,pseudorange,var, keyIndex);
PseudorangeCostFunction* cost_function = new PseudorangeCostFunction(constraint);
std::cout << "keyIndex-> " << keyIndex << std::endl;
for(int i = 0; i <(keyIndex+1); i++)
{
cost_function->AddParameterBlock(5);
}
cost_function->SetNumResiduals(1);
return (cost_function);
}
double s_g_x, s_g_y, s_g_z, pseudorange, var;
int keyIndex;
std::string sat_sys; // satellite system
};
public:
gnssSinglePointPositioning()
{
gnss_raw_sub = nh.subscribe("/gnss_preprocessor_node/GNSSPsrCarRov1", 500, &gnssSinglePointPositioning::gnss_raw_msg_callback, this); // call callback for gnss raw msg
// 回调机制
optimizationThread = std::thread(&gnssSinglePointPositioning::solvePptimization, this);
pub_WLS = nh.advertise<nav_msgs::Odometry>("WLS_spp", 100); // 发布频率为100的里程计
pub_FGO = nh.advertise<nav_msgs::Odometry>("FGO_spp", 100); //
ENULlhRef.resize(3,1);
ENULlhRef<< ref_lon, ref_lat, ref_alt; // 经 纬 高
}
~gnssSinglePointPositioning()
{
optimizationThread.detach();
} // .detach() 开启新的线程
/**
* @brief gnss raw msg callback
* @param gnss raw msg
* @return void
@
*/
void gnss_raw_msg_callback(const nlosExclusion::GNSS_Raw_ArrayConstPtr& msg)
{
m_gnss_raw_mux.lock();
gnss_frame++;
if(msg->GNSS_Raws.size())
{
hasNewData = true;
gnss_raw_buf.push(msg);
gnss_raw_map[gnss_frame] = *msg;
Eigen::MatrixXd eWLSSolutionECEF = m_GNSS_Tools.WeightedLeastSquare(
m_GNSS_Tools.getAllPositions(*msg),
m_GNSS_Tools.getAllMeasurements(*msg),
*msg, "WLS");
Eigen::Matrix<double ,3,1> ENU;
ENU = m_GNSS_Tools.ecef2enu(ENULlhRef, eWLSSolutionECEF);
LOG(INFO) << "ENU WLS -> "<< std::endl << ENU;
nav_msgs::Odometry odometry;
odometry.header.frame_id = "map";
odometry.child_frame_id = "map";
odometry.pose.pose.position.x = ENU(0);
odometry.pose.pose.position.y = ENU(1);
odometry.pose.pose.position.z = ENU(2);
pub_WLS.publish(odometry);
}
m_gnss_raw_mux.unlock();
}
void solvePptimization()
{
while(1)
{
// process gnss raw measurements
// optimize_mux.lock();
if(gnss_raw_map.size() && hasNewData)
{
TicToc optimization_time;
ceres::Problem problem;
ceres::Solver::Options options;
options.use_nonmonotonic_steps = true;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.trust_region_strategy_type = ceres::TrustRegionStrategyType::DOGLEG;
options.dogleg_type = ceres::DoglegType::SUBSPACE_DOGLEG;
options.num_threads = 8; // 线程
options.max_num_iterations = 100; // 最大迭代次数
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
// loss_function = new ceres::HuberLoss(1.0);
loss_function = NULL;
int length = gnss_raw_map.size();
double state_array[length][5]; // ECEF_x, ECEF_y, ECEF_z, ECEF_gps_clock_bias, ECEF_beidou_clock_bias
// std::vector<double*> parameter_blocks;
// std::vector<double*> state_array;
// state_array.reserve(length);
// for(int i = 0; i < length; i++)
// {
// double a[5] = {1,2,3,4,5};
// state_array.push_back(a);
// }
std::map<double, nlosExclusion::GNSS_Raw_Array>::iterator iter;
iter = gnss_raw_map.begin();
for(int i = 0; i < length; i++,iter++) // initialize
{
nlosExclusion::GNSS_Raw_Array gnss_data = (iter->second);
Eigen::MatrixXd eWLSSolutionECEF = m_GNSS_Tools.WeightedLeastSquare(
m_GNSS_Tools.getAllPositions(gnss_data),
m_GNSS_Tools.getAllMeasurements(gnss_data),
gnss_data, "WLS");
state_array[i][0] = 0;
state_array[i][1] = 0;
state_array[i][2] = 0;
state_array[i][3] = 0;
state_array[i][4] = 0;
problem.AddParameterBlock(state_array[i],5);
}
std::map<double, nlosExclusion::GNSS_Raw_Array>::iterator iter_pr;
iter_pr = gnss_raw_map.begin();
for(int m = 0; m < length; m++,iter_pr++) //
{
nlosExclusion::GNSS_Raw_Array gnss_data = (iter_pr->second);
MatrixXd weight_matrix; //goGPS weighting
weight_matrix = m_GNSS_Tools.cofactorMatrixCal_WLS(gnss_data, "WLS"); //goGPS
int sv_cnt = gnss_data.GNSS_Raws.size();
// state_array[m] = new double[5]; //
// double a[5] = {1,2,3,4,5};
// state_array[m] = a;
for(int i =0; i < sv_cnt; i++)
{
std::string sat_sys;
double s_g_x = 0, s_g_y = 0,s_g_z = 0, var = 1;
double pseudorange = 0;
if(m_GNSS_Tools.PRNisGPS(gnss_data.GNSS_Raws[i].prn_satellites_index)) sat_sys = "GPS";
else sat_sys = "BeiDou";
s_g_x = gnss_data.GNSS_Raws[i].sat_pos_x;
s_g_y = gnss_data.GNSS_Raws[i].sat_pos_y;
s_g_z = gnss_data.GNSS_Raws[i].sat_pos_z;
pseudorange = gnss_data.GNSS_Raws[i].pseudorange;
ceres::CostFunction* ps_function = new ceres::AutoDiffCostFunction<pseudorangeFactor, 1
, 5>(new
pseudorangeFactor(sat_sys, s_g_x, s_g_y, s_g_z, pseudorange, sqrt(1/weight_matrix(i,i))));
problem.AddResidualBlock(ps_function, loss_function, state_array[m]);
// pseudorangeConstraint::PseudorangeCostFunction* cost_function =
// pseudorangeConstraint::Create(sat_sys, s_g_x, s_g_y, s_g_z, pseudorange, sqrt(1/weight_matrix(i,i)), m);
// // pseudorange_costFunction->AddParameterBlock(5);
// // pseudorange_costFunction->SetNumResiduals(1);
// std::cout << "state_array size" <<state_array.size()<< "\n";
// problem.AddResidualBlock(cost_function, loss_function, state_array);
}
}
ceres::Solve(options, &problem, &summary);
// std::cout << summary.BriefReport() << "\n";
Eigen::Matrix<double ,3,1> ENU;
Eigen::Matrix<double, 3,1> state;
state<< state_array[length-1][0], state_array[length-1][1], state_array[length-1][2];
ENU = m_GNSS_Tools.ecef2enu(ENULlhRef, state);
LOG(INFO) << "ENU- FGO-> "<< std::endl<< ENU;
nav_msgs::Odometry odometry;
// odometry.header = pose_msg->header;
odometry.header.frame_id = "map";
odometry.child_frame_id = "map";
odometry.pose.pose.position.x = ENU(0);
odometry.pose.pose.position.y = ENU(1);
odometry.pose.pose.position.z = ENU(2);
pub_FGO.publish(odometry);
FILE* FGO_trajectory = fopen("psr_spp_node_trajectory.csv", "w+");
fgo_path.poses.clear();
for(int m = 0; m < length; m++) //
{
state<< state_array[m][0], state_array[m][1], state_array[m][2];
ENU = m_GNSS_Tools.ecef2enu(ENULlhRef, state);
fprintf(FGO_trajectory, "%d,%7.5f,%7.5f,%7.5f \n", m, ENU(0),ENU(1),ENU(2));
fflush(FGO_trajectory);
}
std::cout << "Time used for Ceres-solver-> "<<optimization_time.toc()<<std::endl;
hasNewData = false;
}
std::chrono::milliseconds dura(10); // this thread sleep for 100 ms
std::this_thread::sleep_for(dura);
// gnss_raw_map.clear();
optimize_mux.unlock();
}
}
};
int main(int argc, char **argv)
{
FLAGS_logtostderr = 1; // output to console // 输出到控制台
google::InitGoogleLogging(argv[0]); // init the google logging // 初始化谷歌日志
// google::ParseCommandLineFlags(&argc, &argv, true); // // 初始化 gflags
ros::init(argc, argv, "psr_spp_node");
ROS_INFO("\033[1;32m----> psr_spp_node (solve WLS using Ceres-solver) Started.\033[0m");
gnssSinglePointPositioning gnssSinglePointPositioning;
ros::spin(); // 主程序到这里往下不再进行，等待话题进来回调就行
// 把订阅者的嘴们张开，开始订阅话题并产生回调。
return 0;
}
6649
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
