NormalPrior

class NormalPrior

class NormalPrior: 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, const Vector& b);

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

实现以下形式的成本函数

$$\operatorname{cost}(x)=\|A(x-b)\|^{2}$$

其中，矩阵 $A$ 和向量 $b$ 是固定的，$x$ 是变量。如果用户想要实现以下形式的成本函数

$$\operatorname{cost}(x)=(x-\mu)^{T} S^{-1}(x-\mu)$$

where, $\mu$ is a vector and $S$ is a covariance matrix, then, $A=S^{-1 / 2}$ , i.e the matrix $A$ is the square root of the inverse of the covariance, also known as the stiffness matrix. There are however no restrictions on the shape of $A$ . It is free to be rectangular, which would be the case if the covariance matrix $S$ is rank deficient.