数值分析作业wyj

void Estimator::optimization()
{
    TicToc t_whole, t_prepare;
    vector2double();

    //------------------ 定义问题 定义本地参数化,并添加优化参数-------------------------------------------------
    ceres::Problem problem;// 定义ceres的优化问题
    ceres::LossFunction *loss_function;//核函数
    //loss_function = NULL;
    loss_function = new ceres::HuberLoss(1.0);//HuberLoss当预测偏差小于 δ 时，它采用平方误差,当预测偏差大于 δ 时，采用的线性误差。
    //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++)
    {
        // 对于四元数或者旋转矩阵这种使用过参数化表示旋转的方式，它们是不支持广义的加法
        // 所以我们在使用ceres对其进行迭代更新的时候就需要自定义其更新方式了，具体的做法是实现一个LocalParameterization
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();

        // AddParameterBlock   向该问题添加具有适当大小和参数化的参数块。
        problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization); //因为有四元数,所以使用了 local_parameterization
        if(USE_IMU)
            problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);//使用默认的加法
    }

    // 没使用imu时,将窗口内第一帧的位姿固定
    if(!USE_IMU)
        // SetParameterBlockConstant 在优化过程中，使指示的参数块保持恒定。设置任何参数块变成一个常量
        // 固定第一帧的位姿不变!  这里涉及到论文2中的
        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);//如果是双目,估计两个相机的位姿

        if ((ESTIMATE_EXTRINSIC && frame_count == WINDOW_SIZE && Vs[0].norm() > 0.2) || openExEstimation)
        //Vs[0].norm() > 0.2窗口内第一个速度>2?
        {
            //ROS_INFO("estimate extinsic param");
            openExEstimation = 1;//打开外部估计
        }
        else//如果不需要估计,则把估计器中的外部参数设为定值
        {
            //ROS_INFO("fix extinsic param");
            problem.SetParameterBlockConstant(para_Ex_Pose[i]);
        }
    }

    problem.AddParameterBlock(para_Td[0], 1);//把时间也作为待优化变量
    if (!ESTIMATE_TD || Vs[0].norm() < 0.2)//如果不估计时间就固定
        problem.SetParameterBlockConstant(para_Td[0]);

    // ------------------------在问题中添加约束,也就是构造残差函数---------------------------------- 
    // 在问题中添加先验信息作为约束
    if (last_marginalization_info && last_marginalization_info->valid)
    {
        // 构造新的marginisation_factor construct new marginlization_factor
        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);

        /* 通过提供参数块的向量来添加残差块。
        ResidualBlockId AddResidualBlock(
            CostFunction* cost_function,//损失函数
            LossFunction* loss_function,//核函数
            const std::vector<double*>& parameter_blocks); */
        problem.AddResidualBlock(marginalization_factor, NULL,
                                 last_marginalization_parameter_blocks);
    }

    // 在问题中添加IMU约束
    if(USE_IMU)
    {
        for (int i = 0; i < frame_count; i++)
        {
            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]);
                //这里添加的参数包括状态i和状态j
        }
    }

    int f_m_cnt = 0; //每个特征点,观测到它的相机的计数 visual measurement count
    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 < 4)
            continue;
 
        ++feature_index;

        // imu_i该特征点第一次被观测到的帧 ,imu_j