ceres中的NormalPrior cost function

假设你想估计天线到imu的杆臂这个参数，定义为 $l$，但是在估计的过程中，你不希望这个参数随着给定的数据无所限制的进行更新，因为一旦某些数据是错的，这个估计值就会被错误带偏。如果我们提前进行了机械测量，值为 $l_0$，我们确信无论估计值如何变化，最终的估计值始终在机械测量值附近，那么这个机械测量值就是我们的先验观测。
针对这个prior measurement我们设置一个置信度 $\text{covariance}$ = S = diag { 0.01 , 0.01 , 0.01 } = S = \text{diag}\{0.01, 0.01 ,0.01\} =S=diag{0.01,0.01,0.01}，
则我们的prior measurement构成的factor为

$$\Vert (l-l_0)S^{-1}(l-l_0{)}^T\Vert =\Vert A(x-b){\Vert }^2$$
其中 $A=S^{-\frac{1}{2}},b=l_0,x=l$

这个先验观测的实现如下，注意这个仅限于向量空间的状态估计，如果是SO3这种，需要自定义prior factor。

接口声明

class CERES_EXPORT NormalPrior final : public CostFunction {
 public:
  // Check that the number of rows in the vector b are the same as the
  // number of columns in the matrix A, crash otherwise.
  NormalPrior(const Matrix& A, Vector b);
  bool Evaluate(double const* const* parameters,
                double* residuals,
                double** jacobians) const override;

 private:
  Matrix A_;
  Vector b_;
};



// ------------------------------------- Implementation -----------------------------------
NormalPrior::NormalPrior(const Matrix& A, Vector b) : A_(A), b_(std::move(b)) {
  CHECK_GT(b_.rows(), 0);
  CHECK_GT(A_.rows(), 0);
  CHECK_EQ(b_.rows(), A.cols());
  set_num_residuals(A_.rows());
  mutable_parameter_block_sizes()->push_back(b_.rows());
}

bool NormalPrior::Evaluate(double const* const* parameters,
                           double* residuals,
                           double** jacobians) const {
  ConstVectorRef p(parameters[0], parameter_block_sizes()[0]);
  VectorRef r(residuals, num_residuals());
  // The following line should read
  // r = A_ * (p - b_);
  // The extra eval is to get around a bug in the Eigen library.
  r = A_ * (p - b_).eval();
  if ((jacobians != nullptr) && (jacobians[0] != nullptr)) {
    MatrixRef(jacobians[0], num_residuals(), parameter_block_sizes()[0]) = A_;
  }
  return true;
}