vins estimator marginlization

最新推荐文章于 2023-11-24 23:10:47 发布

xiaoma_bk

最新推荐文章于 2023-11-24 23:10:47 发布

阅读量553

点赞数

分类专栏： # vins 文章标签：计算机视觉人工智能

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/124749715

版权

vins 专栏收录该内容

15 篇文章 3 订阅

订阅专栏

该博客详细介绍了Ceres求解器在优化问题中如何进行边缘化操作，包括ResidualBlockInfo结构、ThreadsStruct线程结构和MarginalizationInfo的使用。边缘化过程涉及计算Jacobian和残差，利用多线程构造Axb系统，并通过舒尔补进行边缘化。此外，还展示了MarginalizationFactor类的核心函数，用于计算边缘化后的Jacobian和残差。

摘要由CSDN通过智能技术生成

参数块信息-ResidualBlockInfo

构造参数
- ceres::CostFunction 通常传入其子类
- ceres::LossFunction 损失函数
  - new ceres::HuberLoss(1.0);
  - new ceres::CauchyLoss(1.0); // 程序使用
- std::vector<double *> _parameter_blocks
  - Eg: vector<double *>{para_Pose[i], para_Pose[j], para_Ex_Pose[0]}等
- std::vector _drop_set 待边缘化的优化变量id
核心成员变量：
- cost_function+loss_function+parameter_blocks+drop_set
- double ** raw_jacobians 雅克比
- std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;
- Eigen::VectorXd residuals; //残差 IMU:15X1 视觉2X1
核心成员函数：
- 调整残差变量的尺寸；同时基于参数块的尺寸，进而构造行Jacobian 和Jacobian的尺寸
- 遍历参数块，为jacobian和行jacobian分配内存
- 求解 Evaluate ，各自代价函数求取
- 若有损失函数，则进行相应的操作

struct ResidualBlockInfo
{
    ResidualBlockInfo(ceres::CostFunction *_cost_function,
                      ceres::LossFunction *_loss_function, std::vector<double *> _parameter_blocks,
                      std::vector<int> _drop_set)
        : cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) {}

    void Evaluate() {
      // 调整残差变量的尺寸
      residuals.resize(cost_function->num_residuals());

      // 参数块的尺寸
      std::vector<int> block_sizes = cost_function->parameter_block_sizes();

      // 定义参数快的行数
      raw_jacobians = new double *[block_sizes.size()];

      // Jacobian 的尺寸
      jacobians.resize(block_sizes.size());

      // 遍历参数块，为jacobian和行jacobian分配内存
      for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)
      {
          jacobians[i].resize(cost_function->num_residuals(), block_sizes[i]);
          raw_jacobians[i] = jacobians[i].data();
          //dim += block_sizes[i] == 7 ? 6 : block_sizes[i];
      }

      // cost_function 估值
      cost_function->Evaluate(parameter_blocks.data(), residuals.data(), raw_jacobians);

      if (loss_function) // 如果损失函数不为空
      {
          double residual_scaling_, alpha_sq_norm_;

          double sq_norm, rho[3];

          // 残差的平方根？
          sq_norm = residuals.squaredNorm();


          loss_function->Evaluate(sq_norm, rho);

          double sqrt_rho1_ = sqrt(rho[1]);

          if ((sq_norm == 0.0) || (rho[2] <= 0.0))
          {
              residual_scaling_ = sqrt_rho1_;
              alpha_sq_norm_ = 0.0;
          }
          else
          {
              const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1];
              const double alpha = 1.0 - sqrt(D);
              residual_scaling_ = sqrt_rho1_ / (1 - alpha);
              alpha_sq_norm_ = alpha / sq_norm;
          }

          for (int i = 0; i < static_cast<int>(parameter_blocks.size()); i++)
          {
              jacobians[i] = sqrt_rho1_ * (jacobians[i] - alpha_sq_norm_ * residuals * (residuals.transpose() * jacobians[i]));
          }

          residuals *= residual_scaling_;
      }
    }

    ceres::CostFunction *cost_function;
    ceres::LossFunction *loss_function;
    std::vector<double *> parameter_blocks;//优化变量数据
    std::vector<int> drop_set;//待边缘化的优化变量id

    double **raw_jacobians;
    std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;
    Eigen::VectorXd residuals;//残差 IMU:15X1 视觉2X1

    int localSize(int size)
    {
        return size == 7 ? 6 : size;
    }
};

线程结构-ThreadsStruct

定义

struct ThreadsStruct
{
    std::vector<ResidualBlockInfo *> sub_factors;
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    std::unordered_map<long, int> parameter_block_size; //global size
    std::unordered_map<long, int> parameter_block_idx; //local size
};

ThreadsConstructA

Function：遍历残差信息块，生成A,b
[i,j,i,j]模块：jacobian_i.transpose() * jacobian_j

static void* ThreadsConstructA(void* threadsstruct)
{
    ThreadsStruct* p = ((ThreadsStruct*)threadsstruct);

    // 遍历残差信息块,生成A b
    for (auto it : p->sub_factors)
    {
        // 遍历每个残差块的 参数块
        for (int i = 0; i < static_cast<int>(it->parameter_blocks.size()); i++)
        {
            // 参数块的下标 和 参数的尺寸
            int idx_i = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[i])];
            int size_i = p->parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[i])];
            if (size_i == 7)  // 若尺寸为7，则赋值 6
                size_i = 6;

            // 当前参数Jacobian 取左 size_i 行
            Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i);
            for (int j = i; j < static_cast<int>(it->parameter_blocks.size()); j++)
            {
                // 给A 和 b 赋值
                int idx_j = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[j])];
                int size_j = p->parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[j])];
                if (size_j == 7)
                    size_j = 6;
                Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j);
                if (i == j)
                    p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
                else
                {
                    p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
                    p->A.block(idx_j, idx_i, size_j, size_i) = p->A.block(idx_i, idx_j, size_i, size_j).transpose();
                }
            }
            p->b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;
        }
    }
    return threadsstruct;
}

边缘化信息-MarginalizationInfo

成员变量

std::vector<ResidualBlockInfo *> factors;//所有观测项
int m, n;//m为要边缘化的变量个数，n为要保留下来的变量个数
std::unordered_map<long, int> parameter_block_size; //<优化变量内存地址,localSize>
std::unordered_map<long, int> parameter_block_idx; //<待边缘化的优化变量内存地址,在parameter_block_size中的id>
std::unordered_map<long, double *> parameter_block_data;//<优化变量内存地址,数据>
std::vector<int> keep_block_size; //global size
std::vector<int> keep_block_idx; //local size
Jacobian 和 residuals：
Eigen::MatrixXd linearized_jacobians;
Eigen::VectorXd linearized_residuals;

调用

构造：由未定义构造即默认构造，new MarginalizationInfo()
首先添加残差块 addResidualBlockInfo
预边缘化 preMarginalize ，可看其： cost time
边缘化 marginalize，可看其： cost time
调整参数块在下一次窗口中对应的位置（去掉次新帧）
- 该步不调用其函数，只是移动外部滑窗数据
得到边缘化信息getParameterBlocks(addr_shift)，为下次添加边缘化因子做准备

Function

addResidualBlockInfo

添加残差块相关信息（优化变量，待边缘化变量）
取代价函数中参数块信息
- 若正常参数块，参数个数一直增大
- 若边缘化参数块，从 0 开始

Code实现：

void addResidualBlockInfo(ResidualBlockInfo *residual_block_info) {
  factors.emplace_back(residual_block_info);

  // 优化变量数据
  std::vector<double *> &parameter_blocks = residual_block_info->parameter_blocks;
  std::vector<int> parameter_block_sizes =
      residual_block_info->cost_function->parameter_block_sizes();

  for (int i = 0; i < static_cast<int>(residual_block_info->parameter_blocks.size()); i++)
  {
      double *addr = parameter_blocks[i];
      int size = parameter_block_sizes[i];
      parameter_block_size[reinterpret_cast<long>(addr)] = size;
  }

  // 待边缘化的优化变量id
  for (int i = 0; i < static_cast<int>(residual_block_info->drop_set.size()); i++)
  {
      //
      double *addr = parameter_blocks[residual_block_info->drop_set[i]];
      parameter_block_idx[reinterpret_cast<long>(addr)] = 0;
  }
}

preMarginalize

计算每个残差及对应的Jacobian，并更新parameter_block_data

代码实现：

  void preMarginalize() {
	// 遍历   参数块信息
    for(auto it: factors) {
	 //  得到其Jacobian 和 残差
      it->Evaluate();

      std::vector<int> block_sizes = it->cost_function->parameter_block_sizes();
      for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)
      {
          // 参数块 i 的地址
          long addr = reinterpret_cast<long>(it->parameter_blocks[i]);
          int size = block_sizes[i];  // 参数尺寸
          if (parameter_block_data.find(addr) == parameter_block_data.end())
          {
              double *data = new double[size];
              memcpy(data, it->parameter_blocks[i], sizeof(double) * size);
              // 将参数块放入参数地址中
              parameter_block_data[addr] = data;
          }
      }
    }
  }
  ```

marginalize

多线程构造先验项舒尔补AX=b的结构，计算Jacobian和残差
- 计算出需要边缘化的参数的总维度: m，计算出未需要边缘化的维度: n
- 创建各个残差块的 A 和 b，并将所有残差块的A 和 b 整合到一起
- 进行边缘化
  - 取要边缘化的部分 Amm
  - 求特征值及特征向量实对称矩阵可以保证对角化成功 Amm_inv
  - 舒尔补
    A = Arr - Arm * Amm_inv * Amr;
    b = brr - Arm * Amm_inv * bmm;
- 将未边缘化的部分重新计算信息矩阵及 sqrt信息矩阵
- 重新计算 Jacobian 和残差

代码实现：

  void marginalize() {

    // 计算出需要边缘化的参数的 总维度: m
    int pos = 0;
    for (auto &it : parameter_block_idx)
    {
        it.second = pos;
        pos += localSize(parameter_block_size[it.first]);
    }
    m = pos;

    // 计算出未需要边缘化的维度:  n
    for (const auto &it : parameter_block_size)
    {
        if (parameter_block_idx.find(it.first) == parameter_block_idx.end())
        {
            parameter_block_idx[it.first] = pos;
            pos += localSize(it.second);
        }
    }
    n = pos - m;

    Eigen::MatrixXd A(pos, pos);
    Eigen::VectorXd b(pos);
    A.setZero();
    b.setZero();

    pthread_t tids[NUM_THREADS];
    ThreadsStruct threadsstruct[NUM_THREADS];
    int i = 0;

    // 将残差快分别分配给不同的线程
    for (auto it : factors)
    {
        threadsstruct[i].sub_factors.push_back(it);
        i++;
        i = i % NUM_THREADS;
    }

    // 生成 threadsstruct A b
    for (int i = 0; i < NUM_THREADS; i++)
    {

        threadsstruct[i].A = Eigen::MatrixXd::Zero(pos,pos);
        threadsstruct[i].b = Eigen::VectorXd::Zero(pos);
        threadsstruct[i].parameter_block_size = parameter_block_size;
        threadsstruct[i].parameter_block_idx = parameter_block_idx;
        int ret = pthread_create( &tids[i], NULL, ThreadsConstructA ,(void*)&(threadsstruct[i]));
        if (ret != 0)
        {
            LOG(ERROR)<<"pthread_create error";
            CHECK(false);
        }
    }

    // 将 总的 A 和 b 整合起来
    for( int i = NUM_THREADS - 1; i >= 0; i--)
    {
        pthread_join( tids[i], NULL );
        A += threadsstruct[i].A;
        b += threadsstruct[i].b;
    }


    // 取 要边缘化的部分
    Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) +
                                 A.block(0, 0, m, m).transpose());

    // 求特征值及特征向量   实对称矩阵可以保证对角化成功
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);
    Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select
                   (saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose();

    //舒尔补
    Eigen::VectorXd bmm = b.segment(0, m);
    Eigen::MatrixXd Amr = A.block(0, m, m, n);
    Eigen::MatrixXd Arm = A.block(m, 0, n, m);
    Eigen::MatrixXd Arr = A.block(m, m, n, n);
    Eigen::VectorXd brr = b.segment(m, n);
    A = Arr - Arm * Amm_inv * Amr;
    b = brr - Arm * Amm_inv * bmm;

    // 将未边缘化的部分 重新计算 信息矩阵及 sqrt信息矩阵
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);
    Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).
                                        select(saes2.eigenvalues().array(), 0));
    Eigen::VectorXd S_inv = Eigen::VectorXd((saes2.eigenvalues().array() > eps).
                                        select(saes2.eigenvalues().array().inverse(), 0));
    Eigen::VectorXd S_sqrt = S.cwiseSqrt();
    Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();
    // asDiagonal() 将向量视为对角矩阵
    // 重新计算 Jacobian 和残差
    linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
    linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;
  }

GetParameterBlocks

  std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift) {

    std::vector<double *> keep_block_addr;
    keep_block_size.clear();
    keep_block_idx.clear();
    keep_block_data.clear();

    for (const auto &it : parameter_block_idx)
    {
        if (it.second >= m)
        {
            keep_block_size.push_back(parameter_block_size[it.first]);
            keep_block_idx.push_back(parameter_block_idx[it.first]);
            keep_block_data.push_back(parameter_block_data[it.first]);
            keep_block_addr.push_back(addr_shift[it.first]);
        }
    }
    sum_block_size = std::accumulate(std::begin(keep_block_size), std::end(keep_block_size), 0);

    return keep_block_addr;

  }

边缘化因子

构造及成员

MarginalizationInfo* marginalization_info;

构造：

  MarginalizationFactor(MarginalizationInfo* _marginalization_info) :
    marginalization_info(_marginalization_info){

    int cnt = 0;
    for (auto it : marginalization_info->keep_block_size)
    {
        mutable_parameter_block_sizes()->push_back(it);
        cnt += it;
    }
    //printf("residual size: %d, %d\n", cnt, n);
    set_num_residuals(marginalization_info->n);

  }

核心函数

  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {

    int n = marginalization_info->n;
    int m = marginalization_info->m;
    Eigen::VectorXd dx(n);
    for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)
    {
        /// 构建差值，非7位时， 直接 与 保持参数相减，7位时后4位为四元数，采用四元数得差值
        int size = marginalization_info->keep_block_size[i];
        int idx = marginalization_info->keep_block_idx[i] - m;
        Eigen::VectorXd x = Eigen::Map<const Eigen::VectorXd>(parameters[i], size);
        Eigen::VectorXd x0 = Eigen::Map<const Eigen::VectorXd>(marginalization_info->keep_block_data[i], size);
        if (size != 7)
            dx.segment(idx, size) = x - x0;
        else
        {
            dx.segment<3>(idx + 0) = x.head<3>() - x0.head<3>();
            dx.segment<3>(idx + 3) = 2.0 * Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
            if (!((Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).w() >= 0))
            {
                dx.segment<3>(idx + 3) = 2.0 * -Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
            }
        }
    }

    // 基于差值得到 其残差
    Eigen::Map<Eigen::VectorXd>(residuals, n) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx;

    // 计算其Jacobian 边缘化Jacobian + 0
    if (jacobians)
    {

        for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)
        {
            if (jacobians[i])
            {
                int size = marginalization_info->keep_block_size[i], local_size = marginalization_info->localSize(size);
                int idx = marginalization_info->keep_block_idx[i] - m;
                Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[i], n, size);
                jacobian.setZero();
                jacobian.leftCols(local_size) = marginalization_info->linearized_jacobians.middleCols(idx, local_size);
            }
        }
    }
    return true;


  }

xiaoma_bk

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
vins estimator marginlization

文章目录参数块信息-ResidualBlockInfo线程结构-ThreadsStruct定义ThreadsConstructA边缘化信息-MarginalizationInfo成员变量调用FunctionaddResidualBlockInfopreMarginalizemarginalizeGetParameterBlocks边缘化因子构造及成员核心函数参数块信息-ResidualBlockInfo构造参数ceres::CostFunction 通常传入其子类ceres::LossFunct
复制链接

扫一扫

专栏目录