这一部分主要是最后的Problem比较重要。
带条件的代价函数ConditionedCostFunction
这个类使用户可以在使用封装好的代价函数的同时,对残差值加入一定的条件限制。举个例子,现在已经有一个代价函数可以产生N个值,但是用户希望的总代价,不是这N个值的简单的平方和。比如对某个特定残差值项赋予一定的系数来改变其在总残差值中的权重。具体代码如下:
// my_cost_function 生成N个残差值。
CostFunction* my_cost_function = ...
CHECK_EQ(N, my_cost_function->num_residuals());
vector<CostFunction*> conditioners;
// Make N 1x1 cost functions (1 parameter, 1 residual)
// 将其构造成N个1x1的代价函数。
CostFunction* f_1 = ...
conditioners.push_back(f_1);
CostFunction* f_N = ...
conditioners.push_back(f_N);
ConditionedCostFunction* ccf =
new ConditionedCostFunction(my_cost_function, conditioners);
现在ccf
中的residual[i]
(i=0…N-1)会被传递到对应的第i个conditioner中。
ccf_residual[i] = f_i(my_cost_function_residual[i])
然后雅可比矩阵的计算相会相应的被调整。
没看明白条件(权重)到底如何给定。
GradientChecker
该类将代价函数返回的雅可比矩阵与使用有限差分估计的导数进行比较。 它的作用是是作为单元测试的工具,给你比求解器选项中的check_gradients
更细致的控制选项。强制执行的条件是:
∀i,j:Jij−J′ijmaxij(Jij−J′ij)<r ∀ i , j : J i j − J i j ′ m a x i j ( J i j − J i j ′ ) < r
Jij J i j 是代价函数计算出的雅可比矩阵再乘以本地参数化的雅可比矩阵。 J′ij J i j ′ 是有限差分法计算出的雅可比矩阵也乘以本地参数化的雅可比矩阵。 r r 是相对精度。用法如下:
// my_cost_function takes two parameter blocks. The first has a local
// parameterization associated with it.
CostFunction* my_cost_function = ...
LocalParameterization* my_parameterization = ...
NumericDiffOptions numeric_diff_options;
std::vector<LocalParameterization*> local_parameterizations;
local_parameterizations.push_back(my_parameterization);
local_parameterizations.push_back(NULL);
std::vector parameter1;
std::vector parameter2;
// Fill parameter 1 & 2 with test data...
std::vector<double*> parameter_blocks;
parameter_blocks.push_back(parameter1.data());
parameter_blocks.push_back(parameter2.data());
GradientChecker gradient_checker(my_cost_function,
local_parameterizations, numeric_diff_options);
GradientCheckResults results;
if (!gradient_checker.Probe(parameter_blocks.data(), 1e-9, &results) {
LOG(ERROR) << "An error has occurred:\n" << results.error_log;
}
NormalPriorr
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;
};
这个类实现了一个如下形式的代价函数:
矩阵 A A 和向量 是固定的。 x x 是变量。如果用户对实现一个如下形式的代价函数感兴趣
其中, μ μ 是向量而 S S 是一个协方差矩阵,那么