g2o中如果没有定义这个边的
linearizeOplus()
,就会调用数值求导。但是g2o的数值求导比较慢,效果较差。所以下面探讨在g2o中嵌入ceres的自动求导,避免复杂的雅可比矩阵的推导。
数值求导与自动求导的区别:
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.
二元数
二元数(Dual number)是实数的推广,二元数引入了一个无穷小的二元数单位:
ε
ε
ε,它的平方
ε
2
=
0
ε^2 = 0
ε2=0(亦即
ε
ε
ε是幂零元),每一个二元数
z
z
z 都有
z
=
a
+
b
ε
z=a+bε
z=a+bε 的特性,其中
a
a
a 和
b
b
b 是实数,
a
a
a 是实部,
b
b
b 是无穷小部。
求雅可比
Jet 是指可微函数
f
f
f 的泰勒展开式中前
k
k
k 项。
假设存在一个Jet,它是一个
n
n
n维的二元数,由一个实部
a
a
a 和
n
n
n 个无穷小的二元数单位(
ϵ
i
,
i
=
1
,
.
.
.
,
n
ϵ_i, i=1,...,n
ϵi,i=1,...,n,
∀
i
,
j
:
ϵ
i
ϵ
j
=
0
∀i,j :ϵ_iϵ_j=0
∀i,j:ϵiϵj=0)构成
x
=
a
+
∑
j
v
j
ϵ
j
x=a+\sum_jv_jϵ_j
x=a+j∑vjϵj
为避免符号冗杂,简化为
x
=
a
+
v
x=a+\textbf{v}
x=a+v
使用泰勒展开,得到:
f
(
a
+
v
)
=
f
(
a
)
+
D
f
(
a
)
v
f(a+\textbf{v})=f(a)+Df(a)\textbf{v}
f(a+v)=f(a)+Df(a)v
对多维函数,
f
:
R
n
→
R
m
f:R^n→R^m
f:Rn→Rm,其中,
x
i
=
a
i
+
v
i
x_i=a_i+\textbf{v}_i
xi=ai+vi,
∀
i
=
1
,
.
.
.
,
n
∀i=1,...,n
∀i=1,...,n:
f
(
x
1
,
.
.
.
,
x
n
)
=
f
(
a
1
,
.
.
.
,
a
n
)
+
∑
i
D
i
f
(
a
1
,
.
.
.
,
a
n
)
v
i
f(x_1,...,x_n)=f(a_1,...,a_n)+\sum_i D_if(a_1,...,a_n)\textbf{v}_i
f(x1,...,xn)=f(a1,...,an)+i∑Dif(a1,...,an)vi
令
v
i
=
e
i
\textbf{v}_i=\textbf{e}_i
vi=ei 作为
i
t
h
i^{th}
ith 个标准误差向量,上面的表达式简化为:
f
(
x
1
,
.
.
.
,
x
n
)
=
f
(
a
1
,
.
.
.
,
a
n
)
+
∑
i
D
i
f
(
a
1
,
.
.
.
,
a
n
)
ϵ
i
f(x_1,...,x_n)=f(a_1,...,a_n)+\sum_i D_if(a_1,...,a_n)ϵ_i
f(x1,...,xn)=f(a1,...,an)+i∑Dif(a1,...,an)ϵi
通过提取
ϵ
i
ϵ_i
ϵi 的系数,我们能得到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
n
n
n维的二元数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_;
};
下一篇:Ceres(二)LocalParameterization参数化