这个函数十分重要，大家要认真学
首先讲一下笔者的个人理解，然后放笔者注释的源码给大家看帮助理解
笔者理解该函数的流程是先初始化两个记录时间的变量t_whole, t_prepare，用于计时和记录算法执行时间。然后将格式转为double格式，以方便ceres库处理。然后到了准备阶段：开始构建参数块，先是姿态的参数块，如果使用imu再构建速度偏置参数块，不适用imu的话就只固定第一帧的姿态参数块，以确保系统的参考帧不变。然后根据情况构建相机的参数块，是采用实时估计还是固定是按条件来，然后是时间延迟参数块，处理时间同步问题。再就是如果有上一帧的边缘化信息就将边缘化因子也添加进ceres这个求解的问题里，还有对使用imu的每一帧之间的imu因子添加残差块，这些因子描述了IMU测量的相对运动信息。然后对于不同情况，对每个特征点，构建视觉测量残差块，这里是根据投影信息构建，投影信息包括相机的内参数和外参数，用于将3D特征点投影到2D图像平面上。
接下来是配置阶段，配置求解器的各个选项，如最大迭代次数、线性求解器类型等。然后调用Solve来求解，结果存进summary中。
最后，如果帧数没达到窗口滑动优化的数量，就不进行边缘化操作了，如果达到了，那么说明我们要删去某帧的信息了在优化中，于是我们丢弃最早帧的位姿和速度偏置的参数块，并建立新的边缘化信息，如果使用imu就把imu因子也存进去，还有所有特征点在当前滑窗中第一帧投影信息，都放进去，接着调用preMarginalize()函数和marginalize()完成边缘化，记录一下时间，就将更新一下边缘化信息，当前帧变为前一帧通过addr_shift映射，
还有一种情况是不是删除最早帧，删除刚放进去的那帧，基本是相同的操作，边缘化处理然后更新，不同的应该就是删除哪帧的数据罢了，marginalization_flag == MARGIN_OLD那就是删除最早帧，否则就是删除新添加的帧。
然后我们要知道几件事情：边缘化因子的构建和添加是为了减少计算复杂度，同时保留必要的信息。
当我们决定边缘化掉某些变量时，这些变量与其他变量之间的约束信息会通过边缘化因子进行保留。边缘化因子是通过将边缘化掉的变量积分到一个新的因子中，使得这个因子能够总结这些变量对系统状态的影响，而在后续的优化过程中，边缘化因子会作为一个新的约束条件，继续作用于系统状态。这样，我们虽然减少了需要直接优化的变量数量，但保留了这些变量对系统状态的约束信息。
下面我们看一下笔者注释的源码;
void Estimator::optimization()
{
// 计时器，用于测量整体和准备阶段的时间
TicToc t_whole, t_prepare;
// 将数据从Eigen类型转换为double数组
vector2double();
// 创建一个Ceres优化问题
ceres::Problem problem;
// 定义损失函数，可以用于处理异常值
ceres::LossFunction *loss_function;
//loss_function = NULL;
// 使用Huber损失函数，参数为1.0
loss_function = new ceres::HuberLoss(1.0);
//loss_function = new ceres::CauchyLoss(1.0 / FOCAL_LENGTH);
//ceres::LossFunction* loss_function = new ceres::HuberLoss(1.0);
// 为每一帧的姿态添加参数块，并指定局部参数化
for (int i = 0; i < frame_count + 1; i++)
{
//创建局部参数化对象：PoseLocalParameterization是一个用户定义的类，继承自Ceres的LocalParameterization类，用于定义姿态参数的局部参数化规则
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
//添加姿态参数块：para_Pose[i]表示第i帧的姿态参数。SIZE_POSE是姿态参数块的大小，local_parameterization是上一步创建的局部参数化对象，确保姿态参数在优化过程中保持合法
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
//添加速度和偏置参数块（如果使用IMU）：
if(USE_IMU)
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
}
// 如果不使用IMU，则固定第一帧的姿态参数块
if(!USE_IMU)
problem.SetParameterBlockConstant(para_Pose[0]);
// 为每个摄像头外参添加参数块，并根据情况固定或估计外参
//循环遍历每个相机
for (int i = 0; i < NUM_OF_CAM; i++)
{
//创建局部参数化对象并添加外参参数块
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
//根据条件决定是否估计或固定外参参数：ESTIMATE_EXTRINSIC：是否估计外参参数的标志。
//frame_count == WINDOW_SIZE：当前帧数是否达到了窗口大小、Vs[0].norm() > 0.2：第一个速度向量的模是否大于0.2
//openExEstimation：外参估计是否已经打开
if ((ESTIMATE_EXTRINSIC && frame_count == WINDOW_SIZE && Vs[0].norm() > 0.2) || openExEstimation)
{
//ROS_INFO("estimate extinsic param");
// 估计外参参数
openExEstimation = 1;
}
else
{
//ROS_INFO("fix extinsic param");
// 固定外参参数
problem.SetParameterBlockConstant(para_Ex_Pose[i]);
}
}
// 添加时间延迟参数块
//将时间延迟参数块para_Td[0]添加到优化问题中，参数块大小为1
problem.AddParameterBlock(para_Td[0], 1);
// 根据条件固定时间延迟参数块
//ESTIMATE_TD：是否估计时间延迟参数的标志、Vs[0].norm() < 0.2：第一个速度向量的模是否小于0.2
if (!ESTIMATE_TD || Vs[0].norm() < 0.2)
//将时间延迟参数块固定，使其在优化过程中保持不变。
problem.SetParameterBlockConstant(para_Td[0]);
// 如果有上一次边缘化信息且有效，则构建新的边缘化因子并添加到优化问题中
if (last_marginalization_info && last_marginalization_info->valid)
{
// construct new marginlization_factor
//使用last_marginalization_info构造一个新的MarginalizationFactor对象
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
//将构造的边缘化因子添加到Ceres优化问题的残差块中。NULL表示没有损失函数，last_marginalization_parameter_blocks表示与该因子相关的参数块。
problem.AddResidualBlock(marginalization_factor, NULL,
last_marginalization_parameter_blocks);
}
// 如果使用IMU，为每个帧之间的IMU因子添加残差块
if(USE_IMU)
{
for (int i = 0; i < frame_count; i++)
{
//获取相邻帧的索引：
int j = i + 1;
//如果预积分时间pre_integrations[j]->sum_dt超过10秒，则跳过该帧对，不添加IMU因子。
if (pre_integrations[j]->sum_dt > 10.0)
continue;
//使用预积分数据pre_integrations[j]构造一个新的IMUFactor对象
IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
//将构造的IMU因子添加到Ceres优化问题的残差块中。NULL表示没有损失函数，para_Pose[i]、para_SpeedBias[i]、para_Pose[j]、para_SpeedBias[j]分别表示与IMU因子相关的当前帧和下一帧的姿态和速度偏差参数块。
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}
}
// 遍历每个特征点，添加视觉测量残差块
//分单目和双目
//单目相机：两个不同帧之间的投影约束（ProjectionTwoFrameOneCamFactor）：当特征点在两个不同的帧中被观测到时，可以通过投影约束来优化这两个帧之间的相对位姿。
//双目相机：单帧双目相机的投影约束（ProjectionOneFrameTwoCamFactor）：使用左、右相机的图像坐标以及相机的外参（两个相机之间的相对位置）来优化特征点的三维坐标和相机的外参。
// 两个不同帧之间的双目相机投影约束（ProjectionTwoFrameTwoCamFactor）：结合了不同帧的相对位姿和左右相机的外参，可以提供更强的几何约束，从而提高估计的精度。
int f_m_cnt = 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();
// 如果特征点的观测次数少于4次，则跳过该特征点
if (it_per_id.used_num < 4)
continue;
++feature_index;// 更新特征点索引
// imu_i是特征点开始的帧索引，imu_j用于遍历其他帧
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)
{
// 获取特征点在当前帧中的位置
Vector3d pts_j = it_per_frame.point;
ProjectionTwoFrameOneCamFactor *f_td = new ProjectionTwoFrameOneCamFactor(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);
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]);
} // 添加单目相机的投影约束
// 如果是双目相机，并且当前帧有双目观测
if(STEREO && it_per_frame.is_stereo)
{
// 获取特征点在右相机中的位置
Vector3d pts_j_right = it_per_frame.pointRight;
// 如果不是开始帧
if(imu_i != imu_j)
{
ProjectionTwoFrameTwoCamFactor *f = new ProjectionTwoFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]);
}// 添加双目相机的投影约束
// 如果是开始帧
else
{
ProjectionOneFrameTwoCamFactor *f = new ProjectionOneFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
problem.AddResidualBlock(f, loss_function, para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]);
}// 添加单帧双目相机的投影约束
}
f_m_cnt++;// 更新处理的特征点对的计数
}
}
// 打印视觉测量数量
ROS_DEBUG("visual measurement count: %d", f_m_cnt);
//printf("prepare for ceres: %f \n", t_prepare.toc());
// 配置Ceres求解器选项
//创建一个ceres::Solver::Options对象options来配置求解器的选项
ceres::Solver::Options options;
//设置线性求解器类型为DENSE_SCHUR，适用于结构化稀疏矩阵的求解
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.num_threads = 2;
//设置信赖域策略类型为DOGLEG，这是一种优化算法
options.trust_region_strategy_type = ceres::DOGLEG;
//设置最大迭代次数为NUM_ITERATIONS
options.max_num_iterations = NUM_ITERATIONS;
//options.use_explicit_schur_complement = true;
//options.minimizer_progress_to_stdout = true;
//options.use_nonmonotonic_steps = true;
//根据边缘化标志marginalization_flag，设置求解器的最大运行时间。如果是旧的边缘化（MARGIN_OLD），则时间为SOLVER_TIME的4/5，否则为SOLVER_TIME。
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来记录求解过程的时间
TicToc t_solver;
//创建一个ceres::Solver::Summary对象summary来存储求解的摘要信息
ceres::Solver::Summary summary;
//调用ceres::Solve函数，使用配置的选项options对问题problem进行求解，并将结果存储在summary中。
ceres::Solve(options, &problem, &summary);
//cout << summary.BriefReport() << endl;
//使用ROS_DEBUG打印优化过程中的迭代次数。
ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
//printf("solver costs: %f \n", t_solver.toc());
// 将优化后的参数转换回Eigen类型
double2vector();
//printf("frame_count: %d \n", frame_count);
// 如果帧数不足WINDOW_SIZE，则返回
if(frame_count < WINDOW_SIZE)
return;
// 处理边缘化：处理旧帧数据，以减少优化问题的复杂性和计算量
//初始化了一个计时器对象，用于测量整个边缘化过程的时间消耗。
TicToc t_whole_marginalization;
//检查边缘化标志，确定是否执行边缘化操作
//这里为旧数据边缘化
//尽管在初始的优化阶段已经处理了单目、双目、单帧双目和多帧的投影因子，但随着时间推移和新数据的加入，旧帧的信息可能会变得过时或不再需要。
if (marginalization_flag == MARGIN_OLD)
{
//创建一个 MarginalizationInfo 对象，用于存储边缘化相关的信息。
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
//转为双精度
vector2double();
//如果存在上一次的有效边缘化信息 last_marginalization_info
if (last_marginalization_info && last_marginalization_info->valid)
{
vector<int> drop_set;
//根据条件，选择需要丢弃的参数块索引，将与最早帧相关联的位姿（para_Pose[0]）和速度偏置（para_SpeedBias[0]）的参数块索引加入 drop_set 中。
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
{
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
//构造一个新的边缘化因子
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
//创建一个 ResidualBlockInfo 对象，包含边缘化因子、相关的参数块、以及需要丢弃的索引，然后将其添加到 marginalization_info 中。
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);
marginalization_info->addResidualBlockInfo(residual_block_info);
}
//如果使用IMU (USE_IMU)，并且第一个预积分的时间小于10秒
if(USE_IMU)
{
if (pre_integrations[1]->sum_dt < 10.0)
{
//创建一个IMU因子
IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
//创建一个 ResidualBlockInfo 对象，包含IMU因子、相关的参数块（如位姿和速度偏置），并将其添加到 marginalization_info 中。
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);
}
}
{
int feature_index = -1;
// 遍历所有特征点
for (auto &it_per_id : f_manager.feature)
{
//获得特征点被观测次数
it_per_id.used_num = it_per_id.feature_per_frame.size();
// 如果特征点被使用的帧数小于4，跳过
if (it_per_id.used_num < 4)
continue;
++feature_index;// 特征点索引自增
//初始化imu_i为起始帧索引，imu_j为其前一帧索引。
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
// 如果imu_i不为0，跳过
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)
{
Vector3d pts_j = it_per_frame.point;
// 添加双目单帧投影因子
ProjectionTwoFrameOneCamFactor *f_td = new ProjectionTwoFrameOneCamFactor(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);
// 创建ResidualBlockInfo对象并添加到marginalization_info中
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);
}
if(STEREO && it_per_frame.is_stereo)
{
Vector3d pts_j_right = it_per_frame.pointRight;
if(imu_i != imu_j)
{
// 添加双目双帧投影因子
ProjectionTwoFrameTwoCamFactor *f = new ProjectionTwoFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
// 创建ResidualBlockInfo对象并添加到marginalization_info中
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]},
vector<int>{0, 4});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
else
{
// 添加单帧双目投影因子
ProjectionOneFrameTwoCamFactor *f = new ProjectionOneFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
// 创建ResidualBlockInfo对象并添加到marginalization_info中
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
vector<double *>{para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]},
vector<int>{2});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
}
}
}
TicToc t_pre_margin;
//preMarginalize() 函数预处理边缘化信息
marginalization_info->preMarginalize();
//记录了预处理的时间。
ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc());
TicToc t_margin;
//marginalize() 函数执行真正的边缘化操作
//边缘化通常指将某些旧帧的状态（如姿态、速度等）从优化中剔除，以减少优化问题的复杂性和计算量
marginalization_info->marginalize();
//记录边缘化的时间
ROS_DEBUG("marginalization %f ms", t_margin.toc());
//addr_shift 是一个无序映射，用于将边缘化后的参数块映射回原始优化问题中的参数块
std::unordered_map<long, double *> addr_shift;
//遍历了窗口内的姿态 para_Pose 和速度偏置 para_SpeedBias，并将其映射到其前一帧的参数块。如果使用了IMU，还会处理速度偏置。
for (int i = 1; i <= WINDOW_SIZE; i++)
{
//将第 i 帧的姿态参数块映射到其前一帧的姿态参数块。
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
if(USE_IMU)
//将第 i 帧的速度偏置参数块映射到其前一帧的速度偏置参数块。
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
//对于外部姿态 para_Ex_Pose 和时间延迟 para_Td，直接映射回其自身的参数块。
for (int i = 0; i < NUM_OF_CAM; i++)
// /para_Ex_Pose[i] 表示第 i 个相机的外部姿态参数块，将其直接映射回自身。
addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
//para_Td[0] 表示时间延迟参数块，也将其直接映射回自身
addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
//getParameterBlocks(addr_shift) 根据映射关系 addr_shift 获取边缘化后的参数块列表，这些参数块已经重新组织好，以供后续优化使用。
vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
//更新上一个边缘化信息 last_marginalization_info 和其参数块列表
//旧的边缘化信息在更新之前会被删除，确保内存管理的正确性和避免内存泄漏。
if (last_marginalization_info)
delete last_marginalization_info;
last_marginalization_info = marginalization_info;
last_marginalization_parameter_blocks = parameter_blocks;
}
else
{
// 检查上一次边缘化信息是否存在，并且上一次边缘化参数块中是否包含最后一个姿态参数块 para_Pose[WINDOW_SIZE - 1]
if (last_marginalization_info &&
std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1]))
{
// 创建新的 MarginalizationInfo 对象
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
vector2double();
// 如果上次边缘化信息有效，构造需要丢弃的参数块集合
if (last_marginalization_info && last_marginalization_info->valid)
{
vector<int> drop_set;
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
{
// 断言速度偏置参数块不等于 para_SpeedBias[WINDOW_SIZE - 1]
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);
// 将 ResidualBlockInfo 对象添加到 marginalization_info 中
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];
if(USE_IMU)
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
else
{
// 其他情况，直接映射到自身
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];
if(USE_IMU)
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];
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;
}
}
//printf("whole marginalization costs: %f \n", t_whole_marginalization.toc());
//printf("whole time for ceres: %f \n", t_whole.toc());
}
427
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
