前言
续接上一篇，本本篇接着介绍VIR-SLAM中estimator.cpp/.h文件的函数，尤其是和UWB相关的相比于VINS改动过的函数，仍然以具体功能情况+代码注释的形式进行介绍。
重点函数介绍
优化函数，代码是先优化，后边缘化。
void Estimator::optimization()
{
ceres::Problem problem;
ceres::LossFunction *loss_function;
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0);
for (int i = 0; i < WINDOW_SIZE + 1; i++)//添加各种待优化量X——位姿优化量，还包括最新的第11帧
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
}
for (int i = 0; i < NUM_OF_CAM; i++)//7维、相机IMU外参
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
if (!ESTIMATE_EXTRINSIC)//如果IMU-相机外参不需要标定
{
ROS_DEBUG("fix extinsic param");
problem.SetParameterBlockConstant(para_Ex_Pose[i]);//这个变量固定为constant
}
else
ROS_DEBUG("estimate extinsic param");
}
if (ESTIMATE_TD)//IMU-image时间同步误差，1维，标定同步时间
{
problem.AddParameterBlock(para_Td[0], 1);
//problem.SetParameterBlockConstant(para_Td[0]);
}
TicToc t_whole, t_prepare;
vector2double(); // 因为ceres用的是double数组,所以下面用vector2double做类型转换
if (last_marginalization_info)//添加先验信残差
{
// construct new marginlization_factor, for the prior element.
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL,
last_marginalization_parameter_blocks);
}
for (int i = 0; i < WINDOW_SIZE; i++)//添加IMU残差
{
int j = i + 1;
if (pre_integrations[j]->sum_dt > 10.0)
continue;
IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}
if(USE_UWB && PsLong.size() == (WINDOW_SIZE_LONG) && KNOWN_ANCHOR == 1)//添加UWB残差
{
//add edge for long window
for (int i = 0; i < PsLong.size(); i++)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose_Long[i], SIZE_POSE, local_parameterization);
}
for (int i = 1; i < PsLong.size(); i++)
{
for (int j = 1; j < 5; j++)
{
int neibLink = i-j;
if (neibLink >0)
{
//cout<<" Add residual in pslong "<< i << " "<< neibLink << " pslong size "<< PsLong.size() << " "<< endl;
ceres::CostFunction* cost_function = LongWindowError::Create(PsLong.at(neibLink), PsLong.at(i), RsLong.at(neibLink), RsLong.at(i), LINK_W);
problem.AddResidualBlock(cost_function, NULL, para_Pose_Long[neibLink], para_Pose_Long[i]);
}
}
ceres::CostFunction* cost_function = movingError::Create(PsLong.at(i), MOVE_W);
problem.AddResidualBlock(cost_function, NULL, para_Pose_Long[i]);
}
// //Link between pose in window and long window
for (int i = 0; i < WINDOW_SIZE; i++)
{
for (int j = 1; j <WINDOW_SIZE_LONG; j++)
{
unsigned neibLink = PsLong.size() + i-j;
if (neibLink<PsLong.size())
{
ceres::CostFunction* cost_function = LongWindowError::Create( PsLong.at(neibLink),Ps[i], RsLong.at(neibLink), Rs[i], 1);
problem.AddResidualBlock(cost_function, NULL, para_Pose_Long[neibLink], para_Pose[i]);
}
}
}
//cout<<" Opt with UWB uwb_keymeas size "<<uwb_keymeas.size()<<" ; "<< endl;
double uwbResidual = 0;
for (int i = WINDOW_SIZE+WINDOW_SIZE_LONG-1; i >=0; i--)
{
//cout<<" For uwb meas i-1 "<< i <<" "<< uwb_keymeas.at(i) <<endl;
if (uwb_keymeas.at(i)>1.5)
{
double avgWindow = 4;
std::deque<double> temp_keymeas = uwb_keymeas;
for (int k = 0; k<avgWindow/2; k++)
{
temp_keymeas.push_front(uwb_keymeas.front());
temp_keymeas.push_back(uwb_keymeas.back());
}
int posID = i - WINDOW_SIZE_LONG;
if (posID >= 0)
{
auto st = temp_keymeas.begin()+ i ;
auto ed = temp_keymeas.begin()+ i + avgWindow;
double sum = 0;
int nums = 0;
for( auto iit = st; iit < ed; iit ++)
{
if (*iit != -1)
{
sum+= *iit;
nums ++;
}
}
double average = sum/nums;
//cout<<"avg start "<<*st << " end "<< *ed << " sum "<< sum << " "<< nums << endl;
//double average = std::accumulate(st, ed, 0.0) / avgWindow;
// //double res = Ps[posID].norm()-uwb_keymeas.at(i);
//cout<<" Short window "<< i <<" "<< uwb_keymeas.at(i) << " avg "<< average<<" ; pos " << posID << " (" << para_Pose[posID][0]<<" " << para_Pose[posID][1]<<" " << para_Pose[posID][2]<< " ); " <<endl;
UWBFactor* uwb_factor = new UWBFactor(average,RANGE_W);
//UWBFactor* uwb_factor = new UWBFactor(uwb_keymeas.at(i),RANGE_W);
// //ceres::LossFunction *loss_function;
// //loss_function = new ceres::HuberLoss(2);
problem.AddResidualBlock(uwb_factor, NULL, para_Pose[posID], anchor_pos);
double res = Ps[posID].norm()-average;
uwbResidual+=abs(res);
}
else
{
int idx_in_long = i;
auto st = temp_keymeas.begin()+ i ;
auto ed = temp_keymeas.begin()+ i + avgWindow;
double sum = 0;
int nums = 0;
for( auto iit = st; iit < ed; iit ++)
{
if (*iit != -1)
{
sum+= *iit;
nums ++;
}
}
double average = sum/nums;
/*** Block following module to test algorithm without long window optimization ***/
// if ((idx_in_long%1)==0)
// {
// //double res = PsLong[idx_in_long].norm()-uwb_keymeas.at(i);
// double res = PsLong[idx_in_long].norm()-average;
// //cout<<" Long window " << i <<" "<< uwb_keymeas[i]<< " avg " << average <<" ; pos ("<< idx_in_long << " " << para_Pose_Long[idx_in_long][0]<<" " << para_Pose_Long[idx_in_long][1]<<" " << para_Pose_Long[idx_in_long][2]<< " ); " <<endl;
// UWBFactor* uwb_factor = new UWBFactor(average, RANGE_W);
// //UWBFactor* uwb_factor = new UWBFactor(uwb_keymeas.at(i), RANGE_W);
// //ceres::LossFunction *loss_function;
// //loss_function = new ceres::CauchyLoss(1.0) ;//new ceres::HuberLoss(0.1);
// problem.AddResidualBlock(uwb_factor, NULL, para_Pose_Long[idx_in_long], anchor_pos);
// uwbResidual+=abs(res);
// }
}
}
}
//cout<<">>>>>>Before optimization: Total uwb residuals "<< uwbResidual <<" anchor pos "<< anchor_pos[0] <<" "<< anchor_pos[1] <<" "<< anchor_pos[2] <<" ;"<<endl;
}
//添加视觉残差
int f_m_cnt = 0;
int feature_index = -1;
for (auto &it_per_id : f_manager.feature)//feature是滑动窗口内所有的特征点的集合
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) // 如果这个特征点被观测的次数大于等于2 并且首次观测到该特征点的帧小于滑动窗口倒数第3帧,这个特征点就可以建立一个残差
continue;
++feature_index;
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;//得到观测到该特征点的首帧
Vector3d pts_i = it_per_id.feature_per_frame[0].point;//得到首帧观测到的特征点的归一化相机坐标
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
if (imu_i == imu_j)
{
continue;
}
Vector3d pts_j = it_per_frame.point;//得到第二个特征点
if (ESTIMATE_TD)
{
ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,
it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
/*
double **para = new double *[5];
para[0] = para_Pose[imu_i];
para[1] = para_Pose[imu_j];
para[2] = para_Ex_Pose[0];
para[3] = para_Feature[feature_index];
para[4] = para_Td[0];
f_td->check(para);
*/
}
else
{
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
}
f_m_cnt++;
}
}
ROS_DEBUG("visual measurement count: %d", f_m_cnt);
ROS_DEBUG("prepare for ceres: %f", t_prepare.toc());
if(relocalization_info)
{
//printf("set relocalization factor! \n");
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);
int retrive_feature_index = 0;
int feature_index = -1;
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
++feature_index;
int start = it_per_id.start_frame;
if(start <= relo_frame_local_index)
{
while((int)match_points[retrive_feature_index].z() < it_per_id.feature_id)
{
retrive_feature_index++;
}
if((int)match_points[retrive_feature_index].z() == it_per_id.feature_id)
{
Vector3d pts_j = Vector3d(match_points[retrive_feature_index].x(), match_points[retrive_feature_index].y(), 1.0);
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);
retrive_feature_index++;
}
}
}
}
ceres::Solver::Options options;//设置求解器
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.num_threads = 2;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = NUM_ITERATIONS;
//options.use_explicit_schur_complement = true;
//options.minimizer_progress_to_stdout = true;
//options.use_nonmonotonic_steps = true;
if (marginalization_flag == MARGIN_OLD)
options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
else
options.max_solver_time_in_seconds = SOLVER_TIME;
TicToc t_solver;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
cout << summary.BriefReport() << endl;
ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
ROS_DEBUG("solver costs: %f", t_solver.toc());
double2vector();
// calculate the residual of all uwb measurements
if ( USE_UWB && PsLong.size() == (WINDOW_SIZE_LONG))
{
double uwbResidual = 0;
for (int i = WINDOW_SIZE+WINDOW_SIZE_LONG-1; i >=0; i--)
{
//cout<<" For uwb meas i-1 "<< i <<" "<< uwb_keymeas.at(i) <<endl;
if (uwb_keymeas.at(i)>1.5)
{
int posID = i - WINDOW_SIZE_LONG;
if (posID >= 0)
{
double res = Ps[posID].norm()-uwb_keymeas.at(i);
uwbResidual+=abs(res);
}
else
{
int idx_in_long = i;
if ((idx_in_long%1)==0)
{
double res = PsLong_result[idx_in_long].norm()-uwb_keymeas.at(i);
uwbResidual+=abs(res);
}
}
}
}
//cout<<"after optimization: Total uwb residuals "<< uwbResidual <<" anchor pos "<< anchor_pos[0] <<" "<< anchor_pos[1] <<" "<< anchor_pos[2] <<" ;"<<endl;
}
//Step3：marg部分
//1.把之前存的残差部分加进来
//2.把与当前要marg掉的帧的所有相关残差项加进来,IMU,vision.
//3.preMarginalize-> 调用Evaluate计算所有ResidualBlock的残差和雅克比,parameter_block_data是margniliazation中存参数块的容器
//4.多线程构造Hx=b的结构,H是边缘化后的结果,First Estimate Jacobian,在X0处进行线性化,需要去看!!!!???????????????????????????
//5.marg结束,调整参数块在下一次window中对应的位置
TicToc t_whole_marginalization;
if (marginalization_flag == MARGIN_OLD)
{
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
vector2double();
//! 先验误差会一直保存，而不是只使用一次
//! 如果上一次边缘化的信息存在
//! 要边缘化的参数块是 para_Pose[0] para_SpeedBias[0] 以及 para_Feature[feature_index](滑窗内的第feature_index个点的逆深度)
if (last_marginalization_info)
{
vector<int> drop_set;
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
{// 查询last_marginalization_parameter_blocks中是首帧状态量的序号
if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
last_marginalization_parameter_blocks[i] == para_SpeedBias[0])
drop_set.push_back(i);
}
// construct new marginlization_factor，//! 构造边缘化的的Factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
//添加上一次边缘化的参数块
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);
marginalization_info->addResidualBlockInfo(residual_block_info);
}
{
if (pre_integrations[1]->sum_dt < 10.0)//添加IMU的先验，只包含边缘化帧的IMU测量残差
{
IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL,
vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},
vector<int>{0, 1});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
{//添加视觉的先验，只添加起始帧是旧帧且观测次数大于2的Features
int feature_index = -1;
for (auto &it_per_id : f_manager.feature)//遍历滑窗内所有的Features
{
it_per_id.used_num = it_per_id.feature_per_frame.size();//该特征点被观测到的次数
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))//Feature的观测次数不小于2次，且起始帧不属于最后两帧
continue;
++feature_index;
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;//只选择被边缘化的帧的Features
if (imu_i != 0)
continue;
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
if (imu_i == imu_j)
continue;
Vector3d pts_j = it_per_frame.point;//得到该Feature在起始下的归一化坐标
if (ESTIMATE_TD)
{
ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,
it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f_td, loss_function,
vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]},
vector<int>{0, 3});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
else
{
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},
vector<int>{0, 3});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
}
}
//将三个ResidualBlockInfo中的参数块综合到marginalization_info中
// 计算所有ResidualBlock(残差项)的残差和雅克比,parameter_block_data是参数块的容器
TicToc t_pre_margin;
marginalization_info->preMarginalize();
ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc());
TicToc t_margin;
marginalization_info->marginalize();
ROS_DEBUG("marginalization %f ms", t_margin.toc());
std::unordered_map<long, double *> addr_shift;
for (int i = 1; i <= WINDOW_SIZE; i++)
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
for (int i = 0; i < NUM_OF_CAM; i++)
addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
if (ESTIMATE_TD)
{
addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
}
vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
if (last_marginalization_info)
delete last_marginalization_info;
last_marginalization_info = marginalization_info;
last_marginalization_parameter_blocks = parameter_blocks;
}
else //MARGIN_SECOND_NEW边缘化倒数第二帧
//如果倒数第二帧不是关键帧
//1.保留该帧的IMU测量,去掉该帧的visual,代码中都没有写.
//2.premarg
//3.marg
//4.滑动窗口移动
{
if (last_marginalization_info &&
std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1]))
{
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
vector2double();
if (last_marginalization_info)
{
vector<int> drop_set;
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
{
ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]);
if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1])
drop_set.push_back(i);
}
// construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);
marginalization_info->addResidualBlockInfo(residual_block_info);
}
TicToc t_pre_margin;
ROS_DEBUG("begin marginalization");
marginalization_info->preMarginalize();
ROS_DEBUG("end pre marginalization, %f ms", t_pre_margin.toc());
TicToc t_margin;
ROS_DEBUG("begin marginalization");
marginalization_info->marginalize();
ROS_DEBUG("end marginalization, %f ms", t_margin.toc());
std::unordered_map<long, double *> addr_shift;
for (int i = 0; i <= WINDOW_SIZE; i++)
{
if (i == WINDOW_SIZE - 1)
continue;
else if (i == WINDOW_SIZE)
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
else
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i];
}
}
for (int i = 0; i < NUM_OF_CAM; i++)
addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
if (ESTIMATE_TD)
{
addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
}
vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
if (last_marginalization_info)
delete last_marginalization_info;
last_marginalization_info = marginalization_info;
last_marginalization_parameter_blocks = parameter_blocks;
}
}
ROS_DEBUG("whole marginalization costs: %f", t_whole_marginalization.toc());
ROS_DEBUG("whole time for ceres: %f", t_whole.toc());
}
滑动窗口函数
实际滑动窗口的地方，如果第二最新帧是关键帧的话，那么这个关键帧就会留在滑动窗口中，时间最长的一帧和其测量值就会被边缘化掉；如果第二最新帧不是关键帧的话，则把这帧的视觉测量舍弃掉而保留IMU测量值在滑动窗口中,这样的策略会保证系统的稀疏性。这一部分跟后端非线性优化是一起进行的，这一部分对应的非线性优化的损失函数的先验部分。
void Estimator::slideWindow()
{
TicToc t_margin;
if (marginalization_flag == MARGIN_OLD)
{
double t_0 = Headers[0].stamp.toSec();
back_R0 = Rs[0];
back_P0 = Ps[0];
if (frame_count == WINDOW_SIZE)
{
for (int i = 0; i < WINDOW_SIZE; i++) // Swap equals to remove the oldest one.
{
Rs[i].swap(Rs[i + 1]);
std::swap(pre_integrations[i], pre_integrations[i + 1]);
dt_buf[i].swap(dt_buf[i + 1]);
linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);
angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);
Headers[i] = Headers[i + 1];
Ps[i].swap(Ps[i + 1]);
Vs[i].swap(Vs[i + 1]);
Bas[i].swap(Bas[i + 1]);
Bgs[i].swap(Bgs[i + 1]);
}
// Manually change the newest position as same as the real newest one.
Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];
Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];
Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();
if (true || solver_flag == INITIAL)
{
map<double, ImageFrame>::iterator it_0;
it_0 = all_image_frame.find(t_0);
delete it_0->second.pre_integration;
it_0->second.pre_integration = nullptr;
for (map<double, ImageFrame>::iterator it = all_image_frame.begin(); it != it_0; ++it)
{
if (it->second.pre_integration)
delete it->second.pre_integration;
it->second.pre_integration = NULL;
}
all_image_frame.erase(all_image_frame.begin(), it_0);
all_image_frame.erase(t_0);
}
slideWindowOld();
}
}
else // MARGIN_NEW
{
if (frame_count == WINDOW_SIZE)
{
for (unsigned int i = 0; i < dt_buf[WINDOW_SIZE].size(); i++)
{
double tmp_dt = dt_buf[WINDOW_SIZE][i];
Vector3d tmp_linear_acceleration = linear_acceleration_buf[WINDOW_SIZE][i];
Vector3d tmp_angular_velocity = angular_velocity_buf[WINDOW_SIZE][i];
pre_integrations[WINDOW_SIZE - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);
dt_buf[WINDOW_SIZE - 1].push_back(tmp_dt);
linear_acceleration_buf[WINDOW_SIZE - 1].push_back(tmp_linear_acceleration);
angular_velocity_buf[WINDOW_SIZE - 1].push_back(tmp_angular_velocity);
}
Headers[WINDOW_SIZE - 1] = Headers[WINDOW_SIZE];
Ps[WINDOW_SIZE - 1] = Ps[WINDOW_SIZE];
Vs[WINDOW_SIZE - 1] = Vs[WINDOW_SIZE];
Rs[WINDOW_SIZE - 1] = Rs[WINDOW_SIZE];
Bas[WINDOW_SIZE - 1] = Bas[WINDOW_SIZE];
Bgs[WINDOW_SIZE - 1] = Bgs[WINDOW_SIZE];
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();
slideWindowNew();
}
}
}
// real marginalization is removed in optimization()
void Estimator::slideWindowNew()
{
sum_of_front++;
f_manager.removeFront(frame_count);
if (USE_UWB)
{
uwb_keymeas.pop_back();
//printf("slide window new-> size of uwb_keymeas %d \n", uwb_keymeas.size());
if (KNOWN_ANCHOR == 0 && solver_flag == NON_LINEAR && uwbMeas4AnchorEst.size()>0)
{
uwbMeas4AnchorEst.pop_back();
}
}
}
// real marginalization is removed in optimization()
void Estimator::slideWindowOld()
{
sum_of_back++;
bool shift_depth = solver_flag == NON_LINEAR ? true : false;
if (shift_depth)
{
Matrix3d R0, R1;
Vector3d P0, P1;
R0 = back_R0 * ric[0];
R1 = Rs[0] * ric[0];
P0 = back_P0 + back_R0 * tic[0];
P1 = Ps[0] + Rs[0] * tic[0];
f_manager.removeBackShiftDepth(R0, P0, R1, P1);
}
else
f_manager.removeBack();
if (USE_UWB && uwb_keymeas.size() > WINDOW_SIZE_LONG+WINDOW_SIZE)
{
uwb_keymeas.pop_front();
}
if (USE_UWB && solver_flag == NON_LINEAR)
{
if (KNOWN_ANCHOR == 1)
{
PsLong.push_back(back_P0);
RsLong.push_back(back_R0);
//printf(" slide window old-> size of uwb_keymeas %d \n", uwb_keymeas.size());
if (PsLong.size() > (WINDOW_SIZE_LONG))
{
PsLong.pop_front();
RsLong.pop_front();
//cout<< "SlideWindow Old, cur first Pos"<< back_P0[0]<<" "<< back_P0[1]<<" "<< back_P0[2]<< " uwb meas front "<< uwb_keymeas.front() <<" uwb meas end "<< uwb_keymeas.back() <<" wubmeas size " << uwb_keymeas.size()<<" ;"<<endl;
}
}
else if(KNOWN_ANCHOR == 0)
{
pose4AnchorEst.push_back(Ps[WINDOW_SIZE - 1]);
}
}
}
锚点估计函数，同样在imageprocess函数中调用的。
void Estimator::estimateAnchorPos()
{
if(USE_UWB && KNOWN_ANCHOR == 0 && pose4AnchorEst.size() > POS_SIZE_4_ANCHOR_EST && uwbMeas4AnchorEst.size()>POS_SIZE_4_ANCHOR_EST)
{
cout<<"------------NOW can do Anchor Estimation pose size "<< pose4AnchorEst.size() <<"; uwb meas size "<< uwbMeas4AnchorEst.size()<<" ;"<<endl;
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
//options.minimizer_progress_to_stdout = true;
options.max_solver_time_in_seconds = 3;
options.max_num_iterations = 50*3;
ceres::Solver::Summary summary;
// Add residual blocks, which are uwb factors.
double uwbResidual;
for(int idx = POS_SIZE_4_ANCHOR_EST; idx>0; idx--){
double temprange = uwbMeas4AnchorEst.back();
Eigen::Vector3d tempPose(pose4AnchorEst.back()[0],pose4AnchorEst.back()[1],pose4AnchorEst.back()[2]);
uwbMeas4AnchorEst.pop_back();
pose4AnchorEst.pop_back();
if (temprange>0.5)
{
double res = tempPose.norm()-temprange;
UWBAnchorFactor* anchor_factor = new UWBAnchorFactor(temprange, RANGE_W, tempPose);
//ceres::LossFunction *loss_function
//loss_function = new ceres::CauchyLoss(1.0) ;//new ceres::HuberLoss(0.1);
problem.AddResidualBlock(anchor_factor, NULL, anchor_pos);
uwbResidual+=abs(res);
cout << "range " << temprange<<" pos ("<<tempPose[0]<<", "<< tempPose[1]<<", "<<tempPose[2]<<" )"<<endl;
}
}
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport()<< "\n";
cout<< " Anchor "<< anchor_pos[0] << " "<< anchor_pos[1] << " "<< anchor_pos[2] << " " <<endl;
KNOWN_ANCHOR = 1;
Eigen::Vector3d eigen_anchor(anchor_pos[0],anchor_pos[1],anchor_pos[2]);
ANCHOR_POS = eigen_anchor;
}
}
小结
optimization函数是基于滑动窗口的优化方法最直接的体现，通过processImage函数在process线程中调用。要想理解optimization函数需要对于滑动窗口BA优化的原理搞清楚，对着公式原理，对着VINS的论文来看会好一些。VIR-SLAM是在VINS-mono的基础之上改来的，主要就是添加了uwb传感器进行优化限制vio的漂移，下一步希望根据这个代码进行一些论文的复现工作。
