g2o中如果没有定义这个边的linearizeOplus()
,就会调用数值求导。但是g2o的数值求导比较慢,效果较差。所以下面探讨在g2o中嵌入ceres的自动求导,避免复杂的雅可比矩阵的推导。
参考:http://ceres-solver.org/automatic_derivatives.html
先看一下数值求导与自动求导的区别:
We will now consider automatic differentiation. It is a technique that can compute exact derivatives, fast, while requiring about the same effort from the user as is needed to use numerical differentiation.
关于自动求导、数值求导、解析求导三者的详细区别,参考:https://blog.csdn.net/yizhou2010/article/details/52712202
二元数
二元数(Dual number)是实数的推广,二元数引入了一个无穷小的二元数单位:
,它的平方
(亦即
是幂零元),每一个二元数
都有
的特性,其中
和
是实数,
是实部,
是无穷小部。
求雅可比
Jet 是指可微函数
的泰勒展开式中前
项。
假设存在一个Jet,它是一个
维的二元数,由一个实部
和
个无穷小的二元数单位(
,
)构成
为避免符号冗杂,简化为
使用泰勒展开,得到:
对多维函数, ,其中, , :
令 作为 个标准误差向量,上面的表达式简化为:
通过提取 的系数,我们能得到Jacobian矩阵
举例说明:
使用Rat43的例子说明,代价函数的计算模型如下:
struct Rat43CostFunctor {
Rat43CostFunctor(const double x, const double y) : x_(x), y_(y) {}
template <typename T>
bool operator()(const T* parameters, T* residuals) const {
const T b1 = parameters[0];
const T b2 = parameters[1];
const T b3 = parameters[2];
const T b4 = parameters[3];
residuals[0] = b1 * pow(1.0 + exp(b2 - b3 * x_), -1.0 / b4) - y_;
return true;
}
private:
const double x_;
const double y_;
};
CostFunction* cost_function =
new AutoDiffCostFunction<Rat43CostFunctor, 1, 4>(
new Rat43CostFunctor(x, y));
其中,这里模板参数T
通常为double
,在需要求residual的雅克比时,T=Jet
维的二元数Jet满足下面的运算法则:
template<int N> struct Jet {
double a;
Eigen::Matrix<double, 1, N> v;
};
template<int N> Jet<N> operator+(const Jet<N>& f, const Jet<N>& g) {
return Jet<N>(f.a + g.a, f.v + g.v);
}
template<int N> Jet<N> operator-(const Jet<N>& f, const Jet<N>& g) {
return Jet<N>(f.a - g.a, f.v - g.v);
}
template<int N> Jet<N> operator*(const Jet<N>& f, const Jet<N>& g) {
return Jet<N>(f.a * g.a, f.a * g.v + f.v * g.a);
}
template<int N> Jet<N> operator/(const Jet<N>& f, const Jet<N>& g) {
return Jet<N>(f.a / g.a, f.v / g.a - f.a * g.v / (g.a * g.a));
}
template <int N> Jet<N> exp(const Jet<N>& f) {
return Jet<T, N>(exp(f.a), exp(f.a) * f.v);
}
// This is a simple implementation for illustration purposes, the
// actual implementation of pow requires careful handling of a number
// of corner cases.
template <int N> Jet<N> pow(const Jet<N>& f, const Jet<N>& g) {
return Jet<N>(pow(f.a, g.a),
g.a * pow(f.a, g.a - 1.0) * f.v +
pow(f.a, g.a) * log(f.a); * g.v);
}
使用上面的重载函数,使用Jets
矩阵调用Rat43CostFunctor
,而不是原来doubles
(parameters
的类型时double
),这样就可以计算得到雅可比矩阵了。
class Rat43Automatic : public ceres::SizedCostFunction<1,4> {
public:
Rat43Automatic(const Rat43CostFunctor* functor) : functor_(functor) {}
virtual ~Rat43Automatic() {}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
// Just evaluate the residuals if Jacobians are not required.
//【1】如果不需要雅可比,直接使用double类型的parameters调用Rat43CostFunctor即可
if (!jacobians) return (*functor_)(parameters[0], residuals);
// Initialize the Jets
//【2】初始化Jets
ceres::Jet<4> jets[4];
for (int i = 0; i < 4; ++i) {
jets[i].a = parameters[0][i];
jets[i].v.setZero();
jets[i].v[i] = 1.0;
}
ceres::Jet<4> result;
(*functor_)(jets, &result);
// Copy the values out of the Jet.
//【3】提取残差
residuals[0] = result.a;
//【4】提取雅可比矩阵
for (int i = 0; i < 4; ++i) {
jacobians[0][i] = result.v[i];
}
return true;
}
private:
std::unique_ptr<const Rat43CostFunctor> functor_;
};
<完>
@leatherwang