CostFunctionModeling Non-linear Least Squares2

最新推荐文章于 2023-06-03 01:26:24 发布

zerolover

最新推荐文章于 2023-06-03 01:26:24 发布

阅读量1.2k

点赞数 3

本文介绍了非线性最小二乘问题中的成本函数，特别是NormalPrior的形式，以及如何利用协方差矩阵进行转换。此外，讨论了ceres库中不同的损失函数，如HuberLoss和SoftLOneLoss，用于处理外点影响。还探讨了局部参数化的重要性，特别是在参数过完备的情况下，如何通过正切空间进行优化，并列举了ceres提供的几种局部参数化实例，如旋转和平移的参数化方法。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

NormalPrior
LossFunction
LocalParameterization
AutoDiffLocalParameterization

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;
 };

该cost function具有这种形式： $cost(x) = \left\| A(x-b) \right\|^2$
我们更感兴趣的是： $cost(x) = (x−\mu)^TS^{−1}(x−\mu)$
$\mu$ 是一个向量、 $S$ 为协方差矩阵。因此 $A=S^{−1/2}$ 。

LossFunction

对于最小二乘问题，其中输入项可能包含outliers，因此需要增加lost function来降低这些外点的影响。

class LossFunction {
 public:
  virtual void Evaluate(double s, double out[3]) const = 0;
};

ceres内定义好的loss function有TrivialLoss、HuberLoss、SoftLOneLoss、CauchyLoss、ArctanLoss、TolerantLoss，
另外还有ComposedLoss（组合两个loss function）、ScaledLoss、LossFunctionWrapper。
LossFunctionWrapper可以在优化时修改loss function的scale。

Problem problem;

// Add parameter blocks

CostFunction* cost_function =
    new AutoDiffCostFunction < UW_Camera_Mapper, 2, 9, 3>(
        new UW_Camera_Mapper(feature_x, feature_y));

LossFunctionWrapper* loss_function(new HuberLoss(1.0), TAKE_OWNERSHIP);
problem.AddResidualBlock(cost_function, loss_function, parameters);

Solver::Options options;
Solver::Summary summary;
Solve(options, &problem, &summary);

loss_function->Reset(new HuberLoss(0.5), TAKE_OWNERSHIP);
Solve(options, &problem, &summary);

NOTE：这里还要再看看手册和理论。

LocalParameterization

有时参数x存在overparameterize的问题。如果x存在与流形空间，我们可以对其正切空间进行参数化。例如，三维空间的球是一个二维的流形。在球体上的每个点，与该点相切的平面定义为正切空间。对于定义在球体上的costfunction，给定一个点x，将x沿法线方向移动是没有用的。因此，一个更好的方式是，对改点对应正切空间的二维向量 $\Delta x$ 进行优化，然后将增量才投影回球体。

$x' = x\boxplus \Delta x$
$x′$ 和 $x$ 的维数相同， $\Delta x$ 的维数小于或等于 $x$ 的。 $\boxplus$ 代表了向量的加。满足 $x' = x\boxplus 0$ 。

LocalParameterization类需要实现以下几个函数：

int LocalParameterization::GlobalSize()
int LocalParameterization::LocalSize()
bool LocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
bool LocalParameterization::ComputeJacobian(const double *x, double *jacobian) const
bool MultiplyByJacobian(const double *x, const int num_rows, const double *global_matrix, double *local_matrix) const

其中Plus实现 $x\boxplus \Delta x$
ComputeJacobian实现 $J=\frac{\partial{x \boxplus \Delta x}}{\Delta x} \big| _{\Delta x=0}$ ，J为row major形式。
MultiplyByJacobian实现 local_matrix = global_matrix * jacobian，其中local_matrix为 num_rows * GlobalSize的矩阵，global_matrix为 num_rows * LocalSize 的矩阵。

ceres定义了几个实例：
IdentityParameterization： $x\boxplus \Delta x = x + \Delta x$
SubsetParameterization： $x\boxplus \Delta x = x + \begin{bmatrix} 0 \\ 1\end{bmatrix} \Delta x$
QuaternionParameterization： $x \boxplus \Delta x = \left[cos(\| \Delta x \|), \frac{sin(\| \Delta x \|)}{\| \Delta x \|}\right]∗x$
HomogeneousVectorParameterization： $x \boxplus \Delta x = \left[ \frac{sin(0.5*\| \Delta x \|)}{\| \Delta x \|}, cos(0.5*\| \Delta x \|),\right]∗x$
ProductParameterization：对于 $SE(3)$ ，

ProductParameterization se3_param(new QuaternionParameterization(),
                                  new IdentityTransformation(3));

AutoDiffLocalParameterization

需要定义一个类含有templated operator() (a functor)计算 $x\boxplus \Delta x$ 。

struct QuaternionPlus {
  template<typename T>
  bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
    const T squared_norm_delta =
        delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];

    T q_delta[4];
    if (squared_norm_delta > T(0.0)) {
      T norm_delta = sqrt(squared_norm_delta);
      const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
      q_delta[0] = cos(norm_delta);
      q_delta[1] = sin_delta_by_delta * delta[0];
      q_delta[2] = sin_delta_by_delta * delta[1];
      q_delta[3] = sin_delta_by_delta * delta[2];
    } else {
      // We do not just use q_delta = [1,0,0,0] here because that is a
      // constant and when used for automatic differentiation will
      // lead to a zero derivative. Instead we take a first order
      // approximation and evaluate it at zero.
      q_delta[0] = T(1.0);
      q_delta[1] = delta[0];
      q_delta[2] = delta[1];
      q_delta[3] = delta[2];
    }

    Quaternionproduct(q_delta, x, x_plus_delta);
    return true;
  }
};

LocalParameterization* local_parameterization =
    new AutoDiffLocalParameterization<QuaternionPlus, 4, 3>;
                                                      |  |
                           Global Size ---------------+  |
                           Local Size -------------------+