ceres的常用函数

最新推荐文章于 2024-12-23 15:12:03 发布

原创最新推荐文章于 2024-12-23 15:12:03 发布 · 1.6k 阅读

20 ·

本内容遵循CC 4.0 BY-SA版权协议

标签

#slam #ceres

slam 专栏收录该内容

50 篇文章

订阅专栏

本文介绍了Ceres优化库中的关键概念，如自定义CostFunction（手动计算残差和雅可比）、AutoDiffCostFunction（自动计算雅可比）、不同类型的LossFunction（如HuberLoss、TolerantLoss和CauchyLoss），以及如何设置ParameterBlock、ResidualBlock和Solver选项。详细讲解了如何构建问题、添加参数和残差块，以及如何配置求解器进行优化。

来源：通义千问

CostFunction：

SizedCostFunction：这是一个手动计算残差与雅可比的实现。你需要继承SizedCostFunction类，并重写Evaluate()函数，在其中计算残差和雅可比矩阵。例如，你可以创建一个MyCostFunction类，如下所示：

class MyCostFunction : public SizedCostFunction<3, 2> {
  public:
    virtual bool Evaluate(double const* const* parameters,
                          double* residuals,
                          double** jacobians) const override {
      // 计算残差
      residuals[0] = parameters[0][0] + parameters[1][0];
      residuals[1] = parameters[0][1] + parameters[1][1];
      residuals[2] = parameters[0][2] + parameters[1][2];

      // 计算雅可比矩阵
      if (jacobians != nullptr) {
        jacobians[0][0] = 1;
        jacobians[0][1] = 1;
        jacobians[0][2] = 1;

        jacobians[1][0] = 1;
        jacobians[1][1] = 1;
        jacobians[1][2] = 1;
      }

      return true;
    }
};

AutoDiffCostFunction：这是一个自动计算雅可比、手动计算残差的实现。你只需要提供一个计算残差的函数，Ceres会使用自动微分技术来计算雅可比矩阵。例如，你可以创建一个MyCostFunction类，如下所示：

struct MyFunctor {
  template <typename T>
  bool operator()(const T* const x, const T* const y, T* residual) const {
    residual[0] = x[0] + y[0];
    residual[1] = x[1] + y[1];
    residual[2] = x[2] + y[2];
    return true;
  }
};

void MyCostFunctionWrapper(void* const* x, void* const* y, double* residual,
                           double** jacobian) {
  MyFunctor functor;
  ceres::AutoDiffCostFunction<MyFunctor, 3, 2, 2>::Evaluate(&functor, x, y,
                                                              residual, jacobian);
}

LossFunction：

HuberLoss：这是一种损失函数，用于处理异常值或噪声较大的数据。它在残差较小的情况下接近平方误差，而在残差较大时逐渐变为绝对误差。例如，你可以使用HuberLoss作为损失函数，如下所示：

ceres::LossFunction* loss_function = new ceres::HuberLoss(1.0);
ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<MyCostFunction, 3, 2>(new MyCostFunction());
ceres::ResidualBlockId residual_block_id = problem.AddResidualBlock(cost_function, loss_function, parameter_blocks[0], parameter_blocks[1]);

TolerantLoss：另一种损失函数，具有容忍度（tolerance），在残差小于容忍度时才开始计算损失。例如，你可以使用TolerantLoss作为损失函数，如下所示：

ceres::LossFunction* loss_function = new ceres::TolerantLoss(0.1);
ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<MyCostFunction, 3, 2>(new MyCostFunction());
ceres::ResidualBlockId residual_block_id = problem.AddResidualBlock(cost_function, loss_function, parameter_blocks[0], parameter_blocks[1]);

CauchyLoss：这是一种Cauchy损失函数，其响应随着残差增大而迅速增加。例如，你可以使用CauchyLoss作为损失函数，如下所示：

ceres::LossFunction* loss_function = new ceres::CauchyLoss(1.0);
ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<MyCostFunction, 3, 2>(new MyCostFunction());
ceres::ResidualBlockId residual_block_id = problem.AddResidualBlock(cost_function, loss_function, parameter_blocks[0], parameter_blocks[1]);

LocalParameterization：

AddParameterBlock：添加一个参数块到问题中，需要指定参数的维度。例如，你可以添加两个参数块，如下所示：

ceres::ParameterBlockOrdering* parameter_ordering = problem.AddParameterBlock(parameter_blocks[0], 2);
ceres::ParameterBlockOrdering* parameter_ordering = problem.AddParameterBlock(parameter_blocks[1], 2);

AddParameterBlockConstant：添加一个常量参数块到问题中，除了指定参数的维度外，还需要设置参数的初始值。例如，你可以添加一个常量参数块，如下所示：

ceres::ParameterBlockOrdering* parameter_ordering = problem.AddParameterBlock(parameter_blocks[0], 2);
ceres::ParameterBlockOrdering* parameter_ordering = problem.AddParameterBlock(parameter_blocks[1], 2);
problem.SetParameterBlockConstant(parameter_blocks[0]);

AddResidualBlock：添加残差块到问题中，需要指定成本函数和参数块。例如，你可以添加一个残差块，如下所示：

ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<MyCostFunction, 3, 2>(new MyCostFunction());
ceres::ResidualBlockId residual_block_id = problem.AddResidualBlock(cost_function, nullptr, parameter_blocks[0], parameter_blocks[1]);

Problem：这是Ceres求解器的核心对象，包含了所有参数、残差块和损失函数。
Solver：
- Options：这是求解器的配置选项，包括：
  - minimizer_type：最小化方法，可以是TRUST_REGION（默认）或LINEAR_SEARCH。
  - trust_region_strategy_type：信赖域策略，可以是LEVENBERG_MARQUARDT（默认）、DOGLEG、DENSE_QR等。
  - linear_solver_type：线性求解器类型，可以是DENSE_NORMAL_CHOLESKY、SPARSE_NORMAL_CHOLESKY等。
  - max_num_iterations：最大迭代次数。
  - num_threads：并行计算使用的线程数。
  - minimizer_progress_to_stdout：是否将每一步的优化进度输出到标准输出。
- Summary：求解过程的总结信息，可以通过BriefReport()和FullReport()获取。
Solve()：调用求解器进行优化，返回优化结果。