0. 简介
上一节我们将while内部的IKD-Tree部分全部讲完,下面将是最后一部分,关于后端优化更新的部分。
1. 迭代更新
最后一块主要做的就是,拿当前帧与IKD-Tree建立的地图算出的残差,然后去计算更新自己的位置,并将更新后的结果通过map_incremental函数插入到由ikd-Tree表示的映射中。
// 外参,旋转矩阵转欧拉角
V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I);
fout_pre << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose() << " " << ext_euler.transpose() << " " << state_point.offset_T_L_I.transpose() << " " << state_point.vel.transpose()
<< " " << state_point.bg.transpose() << " " << state_point.ba.transpose() << " " << state_point.grav << endl; //输出预测的结果
if (0) // If you need to see map point, change to "if(1)"
{
// 释放PCL_Storage的内存
PointVector().swap(ikdtree.PCL_Storage);
// 把树展平用于展示
ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
featsFromMap->clear();
featsFromMap->points = ikdtree.PCL_Storage;
}
pointSearchInd_surf.resize(feats_down_size); //搜索索引
Nearest_Points.resize(feats_down_size); //将降采样处理后的点云用于搜索最近点
int rematch_num = 0;
bool nearest_search_en = true; //
t2 = omp_get_wtime();
/*** 迭代状态估计 ***/
double t_update_start = omp_get_wtime();
double solve_H_time = 0;
//迭代卡尔曼滤波更新,更新地图信息
kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time);
state_point = kf.get_x();
euler_cur = SO3ToEuler(state_point.rot);
pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;
geoQuat.x = state_point.rot.coeffs()[0];
geoQuat.y = state_point.rot.coeffs()[1];
geoQuat.z = state_point.rot.coeffs()[2];
geoQuat.w = state_point.rot.coeffs()[3];
double t_update_end = omp_get_wtime();
/******* 发布里程计 *******/
publish_odometry(pubOdomAftMapped);
/*** 向映射kdtree添加特性点 ***/
t3 = omp_get_wtime();
map_incremental();
t5 = omp_get_wtime();
/******* 发布轨迹和点 *******/
publish_path(pubPath);
if (scan_pub_en || pcd_save_en)
publish_frame_world(pubLaserCloudFull);
if (scan_pub_en && scan_body_pub_en)
{
publish_frame_body(pubLaserCloudFull_body);
publish_frame_lidar(pubLaserCloudFull_lidar);
}
// publish_effect_world(pubLaserCloudEffect);
// publish_map(pubLaserCloudMap);
/*** Debug 参数 ***/
if (runtime_pos_log)
{
frame_num++;
kdtree_size_end = ikdtree.size();
aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num;
aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + (t_update_end - t_update_start) / frame_num;
aver_time_match = aver_time_match * (frame_num - 1) / frame_num + (match_time) / frame_num;
aver_time_incre = aver_time_incre * (frame_num - 1) / frame_num + (kdtree_incremental_time) / frame_num;
aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + (solve_time + solve_H_time) / frame_num;
aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1) / frame_num + solve_time / frame_num;
T1[time_log_counter] = Measures.lidar_beg_time;
s_plot[time_log_counter] = t5 - t0; //整个流程总时间
s_plot2[time_log_counter] = feats_undistort->points.size(); //特征点数量
s_plot3[time_log_counter] = kdtree_incremental_time; // kdtree增量时间
s_plot4[time_log_counter] = kdtree_search_time; // kdtree搜索耗时
s_plot5[time_log_counter] = kdtree_delete_counter; // kdtree删除点数量
s_plot6[time_log_counter] = kdtree_delete_time; // kdtree删除耗时
s_plot7[time_log_counter] = kdtree_size_st; // kdtree初始大小
s_plot8[time_log_counter] = kdtree_size_end; // kdtree结束大小
s_plot9[time_log_counter] = aver_time_consu; //平均消耗时间
s_plot10[time_log_counter] = add_point_size; //添加点数量
time_log_counter++;
printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f ave ICP: %0.6f map incre: %0.6f ave total: %0.6f icp: %0.6f construct H: %0.6f \n", t1 - t0, aver_time_match, aver_time_solve, t3 - t1, t5 - t3, aver_time_consu, aver_time_icp, aver_time_const_H_time);
ext_euler = SO3ToEuler(state_point.offset_R_L_I);
fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose() << " " << ext_euler.transpose() << " " << state_point.offset_T_L_I.transpose() << " " << state_point.vel.transpose()
<< " " << state_point.bg.transpose() << " " << state_point.ba.transpose() << " " << state_point.grav << " " << feats_undistort->points.size() << endl;
dump_lio_state_to_log(fp);
}
}
status = ros::ok();
rate.sleep();
}
/**************** save map ****************/
/* 1. make sure you have enough memories
/* 2. pcd save will largely influence the real-time performences **/
if (pcl_wait_save->size() > 0 && pcd_save_en)
{
string file_name = string("scans.pcd");
string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
pcl::PCDWriter pcd_writer;
cout << "current scan saved to /PCD/" << file_name << endl;
pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
}
fout_out.close();
fout_pre.close();
if (runtime_pos_log)
{
vector<double> t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7;
FILE *fp2;
string log_dir = root_dir + "/Log/fast_lio_time_log.csv";
fp2 = fopen(log_dir.c_str(), "w");
fprintf(fp2, "time_stamp, total time, scan point size, incremental time, search time, delete size, delete time, tree size st, tree size end, add point size, preprocess time\n");
for (int i = 0; i < time_log_counter; i++)
{
fprintf(fp2, "%0.8f,%0.8f,%d,%0.8f,%0.8f,%d,%0.8f,%d,%d,%d,%0.8f\n", T1[i], s_plot[i], int(s_plot2[i]), s_plot3[i], s_plot4[i], int(s_plot5[i]), s_plot6[i], int(s_plot7[i]), int(s_plot8[i]), int(s_plot10[i]), s_plot11[i]);
t.push_back(T1[i]);
s_vec.push_back(s_plot9[i]);
s_vec2.push_back(s_plot3[i] + s_plot6[i]);
s_vec3.push_back(s_plot4[i]);
s_vec5.push_back(s_plot[i]);
}
fclose(fp2);
}
return 0;
}
前面获取的传播状态\hat{x_k}和协方差\hat{P_k}对未知状态x_k施加了一个先验高斯分布。具体来说,\hat{P_k}表示以下误差状态的协方差:
式中,Jκ为( \widehat {x}_ {k}^ {\pi } 田 \widehat {x}_ {k}^ {\kappa } )日 \widehat {x} k对\hat{x}^k_k=0处的偏微分。
式中,A(.)^{-1} 定义于公式(6),如下。
如下的δ^Gθ_{I_k}和δ^Iθ{L_k}分别为IMU的姿态和转动的误差状态。
对于第一次迭代(以拓展的卡尔曼滤波器为例),有 \hat{x^κ_k} =x_k, J_κ =I。
除了先验分布外,我们也有一个源于测量(8)的状态分布:
结合(10)的先验分布和(12)的测量模型,可得到状态x_k的后验分布,其等价表示为x^k_k及其最大后验估计:
式中,有||x||^2_M = x^TM^{−1} x。该最大后验估计问题可由下面的迭代卡尔曼滤波器解决:
需注意,卡尔曼增益K计算需要对状态维数矩阵求逆,而不是在之前的工作中使用的测量维数矩阵。上述过程将重复进行,直到收敛(即||x_κ^{(k+1)} 日 x^κ_k||<无穷小)。收敛后的最优状态和协方差估计为:
通过状态更新x_k,第k次扫描中的每个LiDAR点(Lpj)将通过(16)被转换到全局框架:
转换后的LiDAR点{Gp¯j}将被插入到由ikd-Tree表示的映射中。我们的状态估计在算法1中进行了总结。
// 迭代错误状态EKF更新修改为一个特定的系统
void update_iterated_dyn_share_modified(double R, double &solve_time)
{
dyn_share_datastruct<scalar_type> dyn_share;
dyn_share.valid = true;
dyn_share.converge = true;
int t = 0;
//获取上一次的状态和协方差矩阵
state x_propagated = x_;
cov P_propagated = P_;
int dof_Measurement;
Matrix<scalar_type, n, 1> K_h;
Matrix<scalar_type, n, n> K_x;
vectorized_state dx_new = vectorized_state::Zero();
// 最多进行maximum_iter次迭代优化
for (int i = -1; i < maximum_iter; i++)
{
dyn_share.valid = true;
// 计算测量模型方程的雅克比,也就是点面残差的导数 H(代码里是h_x)
h_dyn_share(x_, dyn_share);
// Matrix<scalar_type, Eigen::Dynamic, 1> h = h_dyn_share(x_, dyn_share);
#ifdef USE_sparse
spMt h_x_ = dyn_share.h_x.sparseView();
#else
// 获取测量模型的雅克比d(pos, rot, 0, 0)/dx
Eigen::Matrix<scalar_type, Eigen::Dynamic, 12> h_x_ = dyn_share.h_x;
#endif
double solve_start = omp_get_wtime();
dof_Measurement = h_x_.rows(); // 观测方程个数m
vectorized_state dx; // 定义误差状态
x_.boxminus(dx, x_propagated); //获取误差dx
dx_new = dx; // 用于迭代的误差状态
if (!dyn_share.valid)
{
continue;
}
// 预测得到的误差状态协方差矩阵
//协方差矩阵在迭代过程中不会代入下一次迭代,直到最后一次退出时更新,在迭代过程中更新的只是先验
P_ = P_propagated;
// 这一大段都在求协方差的先验更新,大致上是P=(J^-1)*P*(J^-T)如论文式16~18
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx(idx + i);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx(idx + i);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
// Matrix<scalar_type, n, Eigen::Dynamic> K_;
// Matrix<scalar_type, n, 1> K_h;
// Matrix<scalar_type, n, n> K_x;
/*
if(n > dof_Measurement)
{
K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose()/R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse()/R;
}
else
{
K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose();
}
*/
// 状态维度 n > 测量维度 dof_Measurement
// 如果状态量维度大于观测方程 n > m,不满秩
if (n > dof_Measurement)
{
//#ifdef USE_sparse
// Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x * P_ * h_x.transpose();
// spMt R_temp = h_v * R_ * h_v.transpose();
// K_temp += R_temp;
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
//每一次迭代将重新计算增益K,即论文式18
h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_;
/*
h_x_cur.col(0) = h_x_.col(0);
h_x_cur.col(1) = h_x_.col(1);
h_x_cur.col(2) = h_x_.col(2);
h_x_cur.col(3) = h_x_.col(3);
h_x_cur.col(4) = h_x_.col(4);
h_x_cur.col(5) = h_x_.col(5);
h_x_cur.col(6) = h_x_.col(6);
h_x_cur.col(7) = h_x_.col(7);
h_x_cur.col(8) = h_x_.col(8);
h_x_cur.col(9) = h_x_.col(9);
h_x_cur.col(10) = h_x_.col(10);
h_x_cur.col(11) = h_x_.col(11);
*/
// 重新计算增益矩阵K
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_ = P_ * h_x_cur.transpose() * (h_x_cur * P_ * h_x_cur.transpose() / R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse() / R;
K_h = K_ * dyn_share.h;
K_x = K_ * h_x_cur;
//#else
// K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse();
//#endif
}
else
{
//避免求逆矩阵,K按稀疏矩阵分解的方法如论文式20
#ifdef USE_sparse
// Eigen::Matrix<scalar_type, n, n> b = Eigen::Matrix<scalar_type, n, n>::Identity();
// Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver;
spMt A = h_x_.transpose() * h_x_;
cov P_temp = (P_ / R).inverse();
P_temp.template block<12, 12>(0, 0) += A;
P_temp = P_temp.inverse();
/*
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
h_x_cur.col(0) = h_x_.col(0);
h_x_cur.col(1) = h_x_.col(1);
h_x_cur.col(2) = h_x_.col(2);
h_x_cur.col(3) = h_x_.col(3);
h_x_cur.col(4) = h_x_.col(4);
h_x_cur.col(5) = h_x_.col(5);
h_x_cur.col(6) = h_x_.col(6);
h_x_cur.col(7) = h_x_.col(7);
h_x_cur.col(8) = h_x_.col(8);
h_x_cur.col(9) = h_x_.col(9);
h_x_cur.col(10) = h_x_.col(10);
h_x_cur.col(11) = h_x_.col(11);
*/
K_ = P_temp.template block<n, 12>(0, 0) * h_x_.transpose();
K_x = cov::Zero();
K_x.template block<n, 12>(0, 0) = P_inv.template block<n, 12>(0, 0) * HTH;
/*
solver.compute(R_);
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b);
spMt R_in =R_in_temp.sparseView();
spMt K_temp = h_x.transpose() * R_in * h_x;
cov P_temp = P_.inverse();
P_temp += K_temp;
K_ = P_temp.inverse() * h_x.transpose() * R_in;
*/
#else
cov P_temp = (P_ / R).inverse();
// Eigen::Matrix<scalar_type, 12, Eigen::Dynamic> h_T = h_x_.transpose();
Eigen::Matrix<scalar_type, 12, 12> HTH = h_x_.transpose() * h_x_;
P_temp.template block<12, 12>(0, 0) += HTH;
/*
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
//std::cout << "line 1767" << std::endl;
h_x_cur.col(0) = h_x_.col(0);
h_x_cur.col(1) = h_x_.col(1);
h_x_cur.col(2) = h_x_.col(2);
h_x_cur.col(3) = h_x_.col(3);
h_x_cur.col(4) = h_x_.col(4);
h_x_cur.col(5) = h_x_.col(5);
h_x_cur.col(6) = h_x_.col(6);
h_x_cur.col(7) = h_x_.col(7);
h_x_cur.col(8) = h_x_.col(8);
h_x_cur.col(9) = h_x_.col(9);
h_x_cur.col(10) = h_x_.col(10);
h_x_cur.col(11) = h_x_.col(11);
*/
cov P_inv = P_temp.inverse();
// std::cout << "line 1781" << std::endl;
K_h = P_inv.template block<n, 12>(0, 0) * h_x_.transpose() * dyn_share.h; // (H_T_H + P^-1)^-1 * H^T * h(残差) = K * h
// std::cout << "line 1780" << std::endl;
// cov HTH_cur = cov::Zero();
// HTH_cur. template block<12, 12>(0, 0) = HTH;
K_x.setZero(); // = cov::Zero();
K_x.template block<n, 12>(0, 0) = P_inv.template block<n, 12>(0, 0) * HTH; //(H_T_H + P^-1)^-1 * H_T_H = KH
// K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose();
#endif
}
// K_x = K_ * h_x_;
//由于是误差迭代KF,得到的是误差的最优估计!
Matrix<scalar_type, n, 1> dx_ = K_h + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new; //误差增量后验 K*h + (K*H - I) dx
state x_before = x_; // 加上校正后的误差状态dx_
x_.boxplus(dx_); //根据计算得到的误差增量后验,更新状态量
// 判断迭代是否发散
dyn_share.converge = true;
//判断已收敛的条件是误差的估计值小于阈值
for (int i = 0; i < n; i++)
{
if (std::fabs(dx_[i]) > limit[i])
{
dyn_share.converge = false;
break;
}
}
if (dyn_share.converge)
t++;
if (!t && i == maximum_iter - 2)
{
dyn_share.converge = true;
}
// 迭代完成后更新误差状态协方差矩阵
//结束迭代后,更新协方差矩阵的后验值,大致上是P=(I-K*H)*P,如论文式19
if (t > 1 || i == maximum_iter - 1)
{
L_ = P_;
// std::cout << "iteration time" << t << "," << i << std::endl;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx_(i + idx);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
for (int i = 0; i < n; i++)
{
L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
// if(n > dof_Measurement)
// {
// for(int i = 0; i < dof_Measurement; i++){
// K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
// }
// }
// else
// {
for (int i = 0; i < 12; i++)
{
K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));
}
//}
for (int i = 0; i < n; i++)
{
L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx_(i + idx);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
for (int i = 0; i < n; i++)
{
L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
// if(n > dof_Measurement)
// {
// for(int i = 0; i < dof_Measurement; i++){
// K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
// }
// }
// else
// {
for (int i = 0; i < 12; i++)
{
K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));
}
//}
for (int i = 0; i < n; i++)
{
L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
// if(n > dof_Measurement)
// {
// Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
// h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_;
// /*
// h_x_cur.col(0) = h_x_.col(0);
// h_x_cur.col(1) = h_x_.col(1);
// h_x_cur.col(2) = h_x_.col(2);
// h_x_cur.col(3) = h_x_.col(3);
// h_x_cur.col(4) = h_x_.col(4);
// h_x_cur.col(5) = h_x_.col(5);
// h_x_cur.col(6) = h_x_.col(6);
// h_x_cur.col(7) = h_x_.col(7);
// h_x_cur.col(8) = h_x_.col(8);
// h_x_cur.col(9) = h_x_.col(9);
// h_x_cur.col(10) = h_x_.col(10);
// h_x_cur.col(11) = h_x_.col(11);
// */
// P_ = L_ - K_*h_x_cur * P_;
// }
// else
//{
P_ = L_ - K_x.template block<n, 12>(0, 0) * P_.template block<12, n>(0, 0);
//}
solve_time += omp_get_wtime() - solve_start;
return;
}
solve_time += omp_get_wtime() - solve_start;
}
}
2. 参考链接
https://blog.csdn.net/MWY123_/article/details/124110021
https://zhuanlan.zhihu.com/p/471876531
https://zhuanlan.zhihu.com/p/471876061
https://zhuanlan.zhihu.com/p/476286576
评论(1)
您还未登录,请登录后发表或查看评论