VINS-mono代码阅读 -- 后端优化

最新推荐文章于 2021-08-19 14:25:51 发布
最新推荐文章于 2021-08-19 14:25:51 发布
阅读量418 收藏 2
点赞数
分类专栏： SLAM 文章标签： slam
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_42580307/article/details/108397341
版权

书接上回，初始化完成以后，就是进行非线性优化了，我们来看看这个函数吧。

1. 非线性优化

1） 状态向量

状态向量包括滑窗内的n+1个所有相机的状态，包括位置，朝向，速度，加速度计bias和陀螺仪bias，相机到IMU的外参，m+1个3D点的逆深度：
X = [ x 0 , x 1 , ⋯   , x c b , λ 0 , λ 1 , ⋯   , λ m ] X = [x_0,x_1,\cdots,x_c^b,\lambda_0,\lambda_1,\cdots,\lambda_m] X=[x0​,x1​,⋯,xcb​,λ0​,λ1​,⋯,λm​]
x k = [ p b k w , v b k w , q b k w , b a , b g ] x_k=[p_{b_k}^w,v_{b_k}^w,q_{b_k}^w,b_a,b_g] xk​=[pbk​w​,vbk​w​,qbk​w​,ba​,bg​]
x c b = [ p c b , q c b ] x_c^b=[p_c^b,q_c^b] xcb​=[pcb​,qcb​]

2）目标函数

min ⁡ X = { ∥ r p − J p X ∥ 2 + ∑ k ∈ B ∥ r B ( z ^ b k + 1 b k , X ) ∥ P b k + 1 b k 2 + ∑ ( i , j ) ∈ C ∥ r C ( z ^ l C j , X ) ∥ p l C j 2 } \min_X=\lbrace \Vert r_p-JpX\Vert^2+\sum_{k\in B}\Vert r_B(\hat z_{b_{k+1}}^{b_k},X)\Vert _{P_{b_{k+1}}^{b_k}}^2 +\sum _{(i,j)\in C}\Vert r_C(\hat z_l^{C_j},X) \Vert_{p_l^{C_j}}^2\rbrace Xmin​={∥rp​−JpX∥2+k∈B∑​∥rB​(z^bk+1​bk​​,X)∥Pbk+1​bk​​2​+(i,j)∈C∑​∥rC​(z^lCj​​,X)∥plCj​​2​}
三个残差项分别为：边缘化的先验信息，IMU的测量残差，视觉的重投影残差，三种残差都是用马氏距离表示的。

3）IMU约束

残差：两帧之间PVQ和bias的变化量
在这里插入图片描述
优化变量：
在这里插入图片描述

4）视觉约束

残差：
在这里插入图片描述
优化变量：
在这里插入图片描述

5）边缘化

根据次新帧是否是关键帧，分为两种边缘化策略：通过对比次新帧和次次新帧的视差，来决定marg掉次新帧或者最老帧，

  1. 当次新帧为关键帧时，MARGIN_OLD，将marg掉最老帧，及其看到的路标点和相关联的IMU数据，将其转换为先验信息加到整体的目标函数。
  2. 当次新帧不是关键帧的时候，MARGIN_SECOND_NEW, 我们将直接丢掉次新帧及它的视觉观测边，而不对次新帧进行marg，因为我们认为当前帧和次新帧很相似，直接丢弃不会造成整个约束关系丢失过多信息，但是，要保留次新帧的IMU数据，从而保证IMU预积分的连贯性。

这里仅仅写出了优化的变量和对应的残差，具体的推导和代码的对应，需要单独的写一章出来。

2. 非线性优化的代码

1) 声明和引入鲁棒核函数
	//1. 构建最小二乘问题
    ceres::Problem problem;
	//2. 创建LossFunction对象， lossfunction 用来减少outlier的影响
    ceres::LossFunction *loss_function;
    //loss_function = new ceres::HuberLoss(1.0);
    loss_function = new ceres::CauchyLoss(1.0);
2） 添加要优化的变量 位姿优化量
	//3. 添加优化变量参数块			add vertex of 1.pose	2.speed	3.bias
	//添加要优化的变量：相机位姿、速度、加速度偏差、陀螺仪偏差
    
	for (int i = 0; i < WINDOW_SIZE + 1; i++)
    {
		//定义了四元数的雅可比加法
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        
		//para_Pose[i]中存的是 第i帧的位姿，para_Pose[i]的大小SIZE_POSE是7
		problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
		//SIZE_SPEEDBIAS 值为9
        problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
    }
3）添加要优化的变量 相机到IMU的外参

如果不需要标定，就固定为常量

    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)
        {
            ROS_DEBUG("fix extinsic param");
            problem.SetParameterBlockConstant(para_Ex_Pose[i]);//这个变量固定为常量
        }
        else
            ROS_DEBUG("estimate extinsic param");
    }
4）添加要优化的变量 Td
	//添加要优化的变量 td
    if (ESTIMATE_TD)
    {
        problem.AddParameterBlock(para_Td[0], 1);
        //problem.SetParameterBlockConstant(para_Td[0]);
    }
5） vector2double()

给parameterBlock赋值。

void Estimator::vector2double()
{
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {
        para_Pose[i][0] = Ps[i].x();
        para_Pose[i][1] = Ps[i].y();
        para_Pose[i][2] = Ps[i].z();
        Quaterniond q{Rs[i]};
        para_Pose[i][3] = q.x();
        para_Pose[i][4] = q.y();
        para_Pose[i][5] = q.z();
        para_Pose[i][6] = q.w();

        para_SpeedBias[i][0] = Vs[i].x();
        para_SpeedBias[i][1] = Vs[i].y();
        para_SpeedBias[i][2] = Vs[i].z();

        para_SpeedBias[i][3] = Bas[i].x();
        para_SpeedBias[i][4] = Bas[i].y();
        para_SpeedBias[i][5] = Bas[i].z();

        para_SpeedBias[i][6] = Bgs[i].x();
        para_SpeedBias[i][7] = Bgs[i].y();
        para_SpeedBias[i][8] = Bgs[i].z();
    }
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        para_Ex_Pose[i][0] = tic[i].x();
        para_Ex_Pose[i][1] = tic[i].y();
        para_Ex_Pose[i][2] = tic[i].z();
        Quaterniond q{ric[i]};
        para_Ex_Pose[i][3] = q.x();
        para_Ex_Pose[i][4] = q.y();
        para_Ex_Pose[i][5] = q.z();
        para_Ex_Pose[i][6] = q.w();
    }

    VectorXd dep = f_manager.getDepthVector();
    for (int i = 0; i < f_manager.getFeatureCount(); i++)
        para_Feature[i][0] = dep(i);
    if (ESTIMATE_TD)
        para_Td[0][0] = td;
}
6）先验信息
	//4. 添加残差块   
	/***
	* 每一项都是一个factor，这是ceres的使用方法，创建一个类继承于ceres::CostFunction类
	* 重写Evaluate()函数定义residual的计算形式
	*/
	//添加边缘化的残差信息，第一次进行优化的时候 last_marginalization_info 还为空值
    if (last_marginalization_info)
    {
        // construct new marginlization_factor
		/*AddResidualBlock函数中第一个参数是 costfunction，第二个参数是lossfunction，第三个参数是参数快*/
        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
        problem.AddResidualBlock(marginalization_factor, NULL,
                                 last_marginalization_parameter_blocks);
    }

第一次执行这段代码的时候，没有先验信息，所以这段是不执行的，第二次执行的时候就有了，last_marginalization_info的赋值出现在后面的代码里。

class MarginalizationInfo
{
  public:
    ~MarginalizationInfo();
    int localSize(int size) const;
    int globalSize(int size) const;
	//加残差块相关信息（优化变量，待marg的变量）
    void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);
	//计算每个残差对应的jacobian，并更新parameter_block_data
    void preMarginalize();
	//pos为所有变量的维度，m要marg的变量，n为要保存的变量
    void marginalize();
    std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);
	//所有观测项
    std::vector<ResidualBlockInfo *> factors;
    //m为要marg掉的变量个数，也就是parameter_block_idx的总localSize,以double为单位，VBias为9，PQ为6
	//n为要保留下的优化变量的变量个数 n=localSize(parameter_block_size) - m
	int m, n;
	//<优化变量的内存地址, localSize>  为每个优化变量的变量大小，IMU(7,9,7,9)
    std::unordered_map<long, int> parameter_block_size; //global size
    
	int sum_block_size;
    //<待marg的优化变量内存地址，在parameter_block_index中的id>
	std::unordered_map<long, int> parameter_block_idx; //local size
	
	//<优化变量内存地址，数据> 每个优化块的数据
    std::unordered_map<long, double *> parameter_block_data;

    std::vector<int> keep_block_size; //global size
    std::vector<int> keep_block_idx;  //local size
    std::vector<double *> keep_block_data;
	//边缘化之后从信息矩阵中恢复出来的雅可比矩阵和残差向量
    Eigen::MatrixXd linearized_jacobians;
    Eigen::VectorXd linearized_residuals;
    const double eps = 1e-8;

};

当marg掉最老帧的时候，parameter_block_size为所有变量及其对应的localSize，parameter_block_data为对应的数据（double* 类型的）。下面举例说明，假设最老帧即第0帧观测到了k = 68个路标点那么代码中的parameter_block_size和parameter_block_data两个vector的大小均为：
2 ( V 0 : 1 ) + 11 ( P 0 : 10 ) + 1 ( T b c ) + 1 ( t b ) + 68 ( λ 1 : k ) = 83 2(V_{0:1})+11(P_{0:10})+1(T_{bc})+1(t_b)+68(\lambda_{1:k})=83 2(V0:1​)+11(P0:10​)+1(Tbc​)+1(tb​)+68(λ1:k​)=83
pose=所有变量总localSize为： 2 × 9 + 11 × 6 + 1 × 6 + 1 × 1 + 68 × 1 = 159 2 \times 9+11\times6+1\times 6+1\times 1+68\times 1=159 2×9+11×6+1×6+1×1+68×1=159
在这里插入图片描述
parameter_block_idx为需要marg掉的变量，仍然假设第一帧看到了k=68个路标点，则parameter_block_idx数组的大小为： p a r a m e t e r _ b l o c k _ i d x . s i z e ( ) = 1 ( P 0 ) + 1 ( V 0 ) + 68 ( λ 1 : k ) = 70 parameter\_block\_idx.size()=1(P_0)+1(V_0)+68(\lambda _{1:k})=70 parameter_block_idx.size()=1(P0​)+1(V0​)+68(λ1:k​)=70
MarginalizationInfo::m表示需要marg变量总的loaclSize, 即为
1 × 6 + 1 × 9 + 68 × 1 = 83 1\times 6+ 1\times 9+68\times 1=83 1×6+1×9+68×1=83
那么，MarginalizationInfo::n表示需要保留的变量的总localSIze，为
n = p o s − m = 159 − 83 = 76 n=pos-m=159-83=76 n=pos−m=159−83=76
在这里插入图片描述

7) IMU残差
	//添加IMU测量值残差
    for (int i = 0; i < WINDOW_SIZE; i++)
    {
        int j = i + 1;
        if (pre_integrations[j]->sum_dt > 10.0)
            continue;
        IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
		//AssResidualBlock 函数中的第一个参数是 costfunction 第二个参数是 lossfunction 第三个参数是 参数块
        problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
    }
8) 重投影残差
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 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;
			//ESTIMATED_TD : online estimate time offset between camera and imu
            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]);

            }
            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++;
        }
    }
9）重定位残差
	//重定位残差
    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++;
                }     
            }
        }

    }

3 求解非线性优化

	//创建优化求解器
    ceres::Solver::Options options;
	//设定优化器的solver_type 类型
    options.linear_solver_type = ceres::DENSE_SCHUR;
    //options.num_threads = 2;

	//设定优化使用的方法，这里使用的是置信域类优化算法中的 dogleg
    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;//优化信息
	//9. 开始求解
    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();

4. 边缘化

在这里插入图片描述

在这里只进行了边缘化，没有求解，求解的过程放在了下一轮的优化中。

1） marginalization_flag == MARGIN_OLD
  • 把上一次先验项中的残差项（尺寸为n）传递给当前的先验项，并从中去除需要丢弃的状态量；
        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++)
            {
                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 *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
                                                                           last_marginalization_parameter_blocks,
                                                                           drop_set);

            marginalization_info->addResidualBlockInfo(residual_block_info);
        }
  • 将滑窗内第0帧和第一帧之间的IMU预积分因子（pre_integrations[1]）放到marginalization_info中
        {
            if (pre_integrations[1]->sum_dt < 10.0)
            {
                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);
            }
        }
  • 挑选出第一次观测帧为第0帧的路标点，将对应的多组视觉观测放到marginalization_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();
                if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
                    continue;

                ++feature_index;

                int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
                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;
                    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);
                    }
                }
            }
        }
  • marginalization_info->preMarginalize(): 得到每次IMU和视觉观测（cost_function）对应的参数块(parameter_blocks)，雅可比矩阵(jacobians)，残差值(residuals)
  • marginalization_info->marginalize(): 多线程计算整个先验的参数块，雅可比矩阵和残差值。
        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 
        last_marginalization_info = marginalization_info;
        last_marginalization_parameter_blocks = parameter_blocks;
        
    }//end of if(marginalization_flag == MARGIN_OLD)
2）marginalization_flag == MARGIN_SECOND_NEW

直接扔掉次新帧和它的视觉观测边，不对次新帧进行marg。但是保存了次新帧的IMU数据，从而保证预积分的连贯性。

    else
    {
	/**
	* 当次新帧不是关键帧时，直接剪切掉次新帧和它的视觉边（该帧和路标点之间的关联），而不对次新帧进行marginalize处理
	* 但是要保留次新帧的IMU数据，从而保证IMU预积分的连续性，这样才能积分计算出下一帧的测量值
	*/
        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");
			//计算每次IMU和视觉观测(cost_function)对应参数块(parameters_block),雅可比矩阵(jacobians)，残差值(residuals)
            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;
            
        }
    }

优化部分到此结束。

半亩园
关注
  • 0
    点赞
  • 踩
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
VINS-Mono代码注释.7z
09-03
这个压缩包"VINS-Mono代码注释.7z"包含了VINS-Mono算法的源代码以及相关的注释，对于理解和实现这类技术非常有帮助。 首先，VINS-Mono的核心在于融合视觉信息（来自摄像头）和惯性信息（来自IMU），以实现高精度的...
vins-mono无ROS版本阅读（8）后端优化
weixin_44754915的博客
03-17 901
参考：【SLAM】VINS-MONO解析——后端优化(代码部分) - 古月居 else//solver_flag = NON_LINEAR,进行非线性优化 { solveOdometry(); //<--1 //边界判断：检测系统运行是否失败，若失败则重置估计器 if (failureDetection()) //<--2 { ROS_WARN("failure detection!");
VINS-Mono 代码解析六、边缘化（2）理论和代码详解
努力努力努力
08-19 2953
1. 边缘化理论 边缘化相关的理论主要是参考高博和贺博的课程以及三篇参考文献： 《The Humble Gaussian Distribution》 《Exactly Sparse Extended Information Filters for Feature-Based SLAM》 《Consistency Analysis for Sliding-Window Visual Odometry》 1.1 为什么要进行边缘化操作？ 首先我们知道，如果仅仅从前后两帧图像来计算相机变换位姿，
VINS-Mono 代码解析六、边缘化（3）
努力努力努力
11-12 5217
一、理论部分 理论部分就不详细说了，看这个连接： https://blog.csdn.net/hltt3838/article/details/109455065 简单介绍 1、优化变量： para_Pose （6维，相机位姿）、para_SpeedBias（9维，相机速度、加速度偏置、角速度偏置）、para_Ex_Pose （6维、相机IMU外参）、para_Feature （1维，特征点深度）、para_Td （1...
vins博客的一部分8
不知语冰
12-29 333
Estimator::optimization() 优化先验残差、重投影残差、预积分残差。（即要拟合的目标是，之前边缘化后的先验值，前后帧之间的IMU的预积分值，每一个特征点的重投影位置。） 首先通过一个函数vector2double()把vector转为double(ceres支持),然后并把Rs Ps Vs Bas…的数据提出来存到para_Pose para_SpeedBias …中 1 定义问题和核函数 ceres::Problem problem;// 定义ceres的优化问题 ceres::Lo
vins中紧耦合优化模型（状态量状态方程观测方程）
沧海一帆的专栏
06-06 1383
问题：vins中误差方程包括哪些？ vins中整体误差方程包含视觉重投影误差、IMU测量误差和先验误差。 优化的factor有 imu_factor.h,marginalization_factor.h,projection_factor.h 问题：vin中优化的状态量是什么？ １.imu测量误差 在imu_factor.h中 class IMUFactor : public ceres::Siz...
VINS-Mono代码注释，仅供学习
01-02
标题中的“VINS-Mono代码注释”意味着这是一个包含了VINS-Mono源代码及其详细解释的学习资源。这样的资料对于深入理解VINS-Mono的工作原理和算法细节至关重要。通过阅读和分析代码注释，SLAM算法工程师可以了解到...
VINS-Mono代码注释以及公式推导
01-17
VINS-Mono代码注释以及公式推导
适用于Ubuntu20.04的VINS-mono源码
05-22
适用于Ubuntu20.04的VINS-mono源码，与官方源码比，修复了在Ubuntu20.04上编译时出现CV_FONT_HERSHEY_SIMPLEX未定义、‘CV_RGB2GRAY’ was not declared等错误。
VINS-Mono代码详细注释
08-14
VINS-Mono是一种基于单目视觉的即时定位与建图...通过理解和学习VINS-Mono的代码，可以深入了解SLAM算法，尤其是视觉惯性融合方面的技术，这对于从事机器人导航、自动驾驶或无人机领域的研究人员和工程师来说极其宝贵。
【SLAM】VINS-MONO解析——后端优化(代码部分)
iwanderu的博客
03-08 5614
7.2 代码 在estimator.cpp的processImage()的最后，代码如下： else//solver_flag = NON_LINEAR,进行非线性优化 { solveOdometry(); //<--1 //边界判断：检测系统运行是否失败，若失败则重置估计器 if (failureDetection()) /...
VINS-Mono 代码解析三、后端优化 第1部分
努力努力努力
11-02 2108
【5】后端优化 一、理论部分 1、需要优化的状态向量 状态向量共包括滑动窗口内的 n+1 个所有相机的状态(包括位置、朝向、速度、加速度计 bias 和陀螺仪 bias)、Camera 到 IMU 的外参、m+1 个 3D 点的逆深度: 2、要优化的目标函数 其中三个残差项即误差项分别为 边缘化的先验信息、IMU 测量残差、视觉的重投影残差，三种残差都是用马氏距离表示；对于 边缘化的先验信息 可能有点模糊，有面我们在仔细的分析！ 用下面的一个经典的图去理解 视觉 IMU的约束...
数值分析作业wyj
yoga_wyj的博客
10-30 243
void Estimator::optimization() { TicToc t_whole, t_prepare; vector2double(); //------------------ 定义问题 定义本地参数化,并添加优化参数------------------------------------------------- ceres::Problem problem;// 定义ceres的优化问题 ceres::LossFunction *loss_f.
vins中imu融合_VINS代码解读
weixin_28871743的博客
12-30 897
VINS_estimator摘抄我们初始化的原因是单目惯性紧耦合系统是一个非线性程度很高的系统，首先单目是无法获得空间中的绝对尺度，而IMU又必然存在偏置，在后面进行求解的时候还需要用到重力加速度(包括大小和方向)，对于速度比较敏感的条件下，比如说无人机，又要精确的速度信息，因此，如何有效的在紧耦合系统处理之前计算出这些量，对整个紧耦合系统的鲁棒性有着重大的意义(其实这里就可以理解成相机标定一样，...
VINS-Mono源码分析4— vins_estimator3(非线性优化)
yys2324826380的博客
04-06 759
VINS-Mono源码分析4— vins_estimator3
VINS-Mono 代码详细解读——基于滑动窗口的紧耦合后端非线性优化 optimization()+滑动窗slideWindow()
try_again_later的博客
03-21 4970
上一篇文章是基于滑动窗的紧耦合后端非线性优化的理论部分，主要分为以下部分： VIO残差函数构建 视觉约束：视觉重投影误差residual、残差对状态量的Jacobian、协方差 IMU约束：residual、Jacobian、Covariance 基于舒尔补的边缘化 本文将对Estimator::optimization()函数的代码进行详细讲解。 目录 一、VIO残差函数构建...
VINS-Mono代码阅读笔记（十）：vins_estimator中的非线性优化
12-23 4027
本篇笔记紧接着VINS-Mono代码阅读笔记（九）：vins_estimator中的相机-IMU对齐，初始化完成之后就获得了要优化的变量的初始值，后边就是做后端优化处理了。这部分对应论文中第VI部分，紧耦合的单目VIO，这部分也是该系统的核心部分。 1.前言 在我们对经典的视觉SLAM框架和现有的知识一无所知的情况下，我们看到搭载有相机的移动设备（无人车或者无人机等）从A地点运动到B地点，我们...
VINS-Mono 代码解读
热门推荐
u012871872的博客
09-28 2万+
VINS-Mono 代码解读标签（空格分隔）： SLAM VIO系统启动命令$roslaunch vins_estimator euroc.launch$roslaunch vins_estimator vins_rviz.launch $rosbag play YOUR_PATH_TO_DATASET/MH_03_medium.bag $roslaunch benchmark_publisher
VINS-Mono代码解读——状态估计器流程 estimator 写在初始化和非线性优化前
Manii
01-13 1万+
前言 本文主要介绍VINS的状态估计器模块（estimator），或者从ROS角度说是estimator节点。 这个模块可以说是VINS的最核心模块，从论文的内容上来说，里面的内容包括了VINS的估计器初始化、基于滑动窗口的非线性优化实现紧耦合，即论文第五章（V. ESTIMATOR INITIALIZATION）第六章（VI. TIGHTLY-COUPLED MONOCULAR VIO）。 代码...
vins-mono 代码解读
最新发布
07-28
5. 优化：在 IMU 数据的基础上，vins-mono 使用非线性优化算法（如基于因子图的优化方法）来最小化重投影误差，并优化相机的轨迹和3D点的位置。 6. 地图更新：随着新的图像和IMU数据的到来，vins-mono 不断更新地图...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值