ceres solver Non-linear Least Squares

1. 簡介

ceres solver

1.1 基本概念

ceres 可以解決形式受限的魯棒非線性最小二乘問題：
$\underset{x}{min}\frac{1}{2} \sum_i \rho _i(\left \| f_i(x_{i_1},...,x_{i_k}) \right \|^2)$${s.t. l_j \leq x_j \leq u_j}$ 在表達式中：${\rho _i(\left \| f_i(x_{i_1},...,x_{i_k}) \right \|^2}$是作爲已知量的殘差塊。

• ${f_i(\cdot)}$是一個依賴參數塊${[x_{i_1},...,x_{i_k}]}$的代價函數。
• ${\rho _i}$是損失函數。損失函數：是一個標量函數，用於減少離羣值對非線性最小二乘問題解的影響。

作爲一種特殊情況，當${\rho _i(x)=x}$時，即恆等函數，以及${l_j = -∞,u_j = ∞}$，我們得到了更爲熟悉的非線性最小二乘問題。

1.2 案例

求最小值： ${\frac{1}{2} (x-10)^2}$

1. 首先寫一個函數，評估上述的函數：${f(x) = (x-10)}$
2. 一旦我們有了一種計算殘差函數的方法，現在就可以使用它來構造非線性最小二乘問題，並讓Ceres解決它。

struct CostFunctor {
   template <typename T>
   bool operator()(const T* const x, T* residual) const {
     residual[0] = T(10.0) - x[0];
     return true;
   }
};

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);

  // The variable to solve for with its initial value.
  double initial_x = 5.0;
  double x = initial_x;

  // Build the problem.
  Problem problem;

  // Set up the only cost function (also known as residual). This uses
  // auto-differentiation to obtain the derivative (jacobian).
  CostFunction* cost_function =
      new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
  problem.AddResidualBlock(cost_function, NULL, &x);

  // Run the solver!
  Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;
  Solver::Summary summary;
  Solve(options, &problem, &summary);

  std::cout << summary.BriefReport() << "\n";
  std::cout << "x : " << initial_x
            << " -> " << x << "\n";
  return 0;
}

• AutoDiffCostFunction將CostFunctor作爲輸入，對其進行自動區分併爲其提供CostFunction接口。

1.3 衍生

像大多數優化程序包一樣，Ceres Solver依賴於能夠在任意參數值下評估目標函數中每個項的值和導數。 正確有效地執行此操作對於獲得良好的結果至關重要。 Ceres Solver提供了許多方法。
現在我們考慮另外兩種可能性。解析和數值導數。

1.3.1 數值導數

在某些情況下，無法定義模板化的成本函子，例如，當殘差的評估涉及對您無法控制的庫函數的調用時。 在這種情況下，可以使用數值微分。 用戶定義一個函子，該函子可計算殘差值並使用其構造一個NumericDiffCostFunction。 例如，對於f（x）= 10-x，相應的函子將爲：

struct NumericDiffCostFunctor {
  bool operator()(const double* const x, double* residual) const {
    residual[0] = 10.0 - x[0];
    return true;
  }
};

將以下內容添加到問題中：

CostFunction* cost_function =
  new NumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL, 1, 1>(
      new NumericDiffCostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);

注意我們使用自動微分時的相似之處：

CostFunction* cost_function =
    new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);

該構造看上去幾乎與用於自動微分的構造相同，除了一個額外的模板參數，該參數指示了用於計算數值導數的有限微分方案的類型[3]。 有關更多詳細信息，請參見NumericDiffCostFunction的文檔。
一般來說，我們建議使用自動微分而不是數值微分。 C ++模板的使用使自動微分有效，而數字微分昂貴，容易出現數字錯誤並導致收斂速度變慢。

1.3.2 解析導數

在某些情況下，無法使用自動區分。 例如，可能存在這樣的情況，即以封閉形式計算導數而不是依靠自動微分代碼使用的鏈式規則更爲有效。
在這種情況下，可以提供自己的殘差和雅可比計算代碼。 爲此，如果您知道編譯時參數和殘差的大小，請定義CostFunction或SizedCostFunction的子類。 例如，這裏是實現f（x）= 10-x的SimpleCostFunction。

class QuadraticCostFunction : public ceres::SizedCostFunction<1, 1> {
 public:
  virtual ~QuadraticCostFunction() {}
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const {
    const double x = parameters[0][0];
    residuals[0] = 10 - x;

    // Compute the Jacobian if asked for.
    if (jacobians != NULL && jacobians[0] != NULL) {
      jacobians[0][0] = -1;
    }
    return true;
  }
};

SimpleCostFunction :: Evaluate提供了一個輸入參數數組，一個用於殘差的輸出數組殘差以及一個用於Jacobian的輸出數組jacobians。 jacobians數組是可選的，Evaluate應該檢查它是否爲非空值，如果是這種情況，則用殘差函數的導數值填充它。 在這種情況下，由於殘差函數是線性的，因此雅可比常數爲常數[4]。
從上面的代碼片段可以看出，實現CostFunction對象有點繁瑣。 我們建議除非您有充分的理由自己管理jacobian計算，否則請使用AutoDiffCostFunction或NumericDiffCostFunction構造殘差塊。

1.4 有關衍生工具的更多信息

到目前爲止，計算導數是使用Ceres的最複雜的部分，並且根據情況，用戶可能需要更復雜的計算導數的方法。 本節僅介紹如何將衍生物提供給Ceres。 一旦您對使用NumericDiffCostFunction和AutoDiffCostFunction感到滿意，我們建議您看一下DynamicAutoDiffCostFunction，CostFunctionToFunctor，NumericDiffFunctor和ConditionedCostFunction，以瞭解構建和計算成本函數的更高級方法。

2. Powell’s Function

現在考慮一個稍微複雜一點的示例-powell’s函數的最小化。讓:
$x=[x_1,x_2,x_3,x_4]$
${f_1(x)=x_1+10x_2}$
${f_2(x)=\sqrt{5}(x_3-x_4)}$
${f_3(x)=(x_2-2x_3)^2}$
${f_4(x)=\sqrt{10}(x_1-x_4)^2}$
${F(x)=[f_1(x),f_2(x),f_3(x),f_4(x)]}$
$F(x)$是四個參數的函數，有四個殘差，我們找$x$使得${\left \| \frac{1}{2}F(x) \right \|^2}$最小。
同樣，第一步是定義對目標函子中的項進行求值的函子。這是評估代碼${f_4(x_1,x_4)}$:

struct F4 {
  template <typename T>
  bool operator()(const T* const x1, const T* const x4, T* residual) const {
    residual[0] = T(sqrt(10.0)) * (x1[0] - x4[0]) * (x1[0] - x4[0]);
    return true;
  }
};

同理，我們定義${F1,F2,F3}$，使用這些，問題可以構造如下：

double x1 =  3.0; double x2 = -1.0; double x3 =  0.0; double x4 = 1.0;

Problem problem;

// Add residual terms to the problem using the using the autodiff
// wrapper to get the derivatives automatically.
problem.AddResidualBlock(
  new AutoDiffCostFunction<F1, 1, 1, 1>(new F1), NULL, &x1, &x2);
problem.AddResidualBlock(
  new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), NULL, &x3, &x4);
problem.AddResidualBlock(
  new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), NULL, &x2, &x3)
problem.AddResidualBlock(
  new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), NULL, &x1, &x4);

請注意，每個ResidualBlock僅取決於相應殘差對象所依賴的兩個參數，而不取決於所有四個參數。