看了 Joan Solà 的论文 [1],写了博客 《A Micro Lie Theory 论文理解》。自认为对论文理解程度已经可以实用。
找一个以前解决得不好的过程用现在学到的理论处理。SLERP(Spherical Linear intERPolation)过程,在 Joan Solà 的论文 [2] Section 2.7 中有介绍 3 种方法,此处选择 Method 1。为了保证问题描述清晰,所以以下定义问题的方法不会与 [2] 中完全一致。
以下正文中的公式引用使用两种形式:本文的公式,如(1);[1] 的公式,如(1*)。
1. 问题定义
假设相机 C 相对世界坐标系 W 的姿态可以用 ({}^{C}mathbf{R}_W) 表示。在 (t_0) 时刻,姿态为 ({}^{C0}mathbf{R}_W),在 (t_1) 时刻,姿态为 ({}^{C1}mathbf{R}_W),(t_0 < t_1)。那么在 (t_0) 时刻到 (t_1) 时刻中间的某个时刻 (t_i) 相机的姿态 ({}^{Ci}mathbf{R}_W) 如何用 ({}^{C0}mathbf{R}_W, {}^{C1}mathbf{R}_W) 表达?
令 (t_i = t_0 + alpha (t_1 - t_0), alphain[0,1])。
求 ({}^{Ci}mathbf{R}_W) 对 ({}^{C1}mathbf{R}_W, {}^{C0}mathbf{R}_W, alpha) 的导数。前两个导数是 (mathbb{R}^3) 对 (mathbb{R}^3) 的导数,最后一个导数是 (mathbb{R}^3) 对 (mathbb{R}) 的导数。
2. 选择
所以,我们需要求什么形式的导数?是扰动还是右扰动?为了方便代码验证,按照我前面一篇博客 《[ceres-solver] From QuaternionParameterization to LocalParameterization》 所述,ceres::QuaternionParameterization
默认是左扰动。依此定下左扰动的 convention。
这个问题是否与 Quaternion 的 Hamilton 或 JPL 有关?公式推导与其无关,公式推导只涉及 SO(3), so(3),以及 so(3) 对应的 (mathbb{R}^3)。
3. 求导
因为是使用左扰动形式,所以对应到论文 [1] 中,主要使用公式 (27*), (28*), (44*)。
以下求导会大量使用 chain rule,不一定对,但是总是可以试一下,最后使用代码验证公式是否正确。使用 chain rule 需要谨记 [1] (59*) 之前的一句话。
Notice that when mixing right, left and corssed Jacobians, we need to chain also the reference frames, ...
令 (m{ au}= ext{Log}({}^{C1}mathbf{R}_W {}^{C0}mathbf{R}_W^T)),则。
对需要计算的三个导数 ({partial {}^{Ci}mathbf{R}_W over partial {}^{C0}mathbf{R}_W}, {partial {}^{Ci}mathbf{R}_W over partial {}^{C1}mathbf{R}_W}, {partial {}^{Ci}mathbf{R}_W over partial alpha}) (虽然导数这么写不严谨,但是也就这样了)应用 chain rule。
以上的公式 ((
ef{eq:wrong})) 是错误的,但是后面按照这个公式计算出来的 ({partial {}^{Ci}mathbf{R}_W over partial {}^{C0}mathbf{R}_W}) 结果 ((
ef{eq:wrong1})) 却是能够通过代码验证(以下4.1
,得到的结果与数值方法的结果一致)。本来是想按照 chain rule 写,即 ((f(x)g(x))^{prime} = f(x)^{prime} g(x) + f(x)g(x)^{prime}),但是中间出了错,应该是写成如下。结果歪打正着,得到了可以通过代码验证的结果。
为什么 (( ef{eq:wrong})) 是错误的?因为导数的乘法法则 ((f(x)g(x))^{prime} = f(x)^{prime} g(x) + f(x)g(x)^{prime}) 是在实数域上成立,并没有在 Manifold 上成立。按照 Manifold 上的导数定义 (44*),应当如下计算,然而这种计算是没有结果的。
那么正确的写法是什么?将 (( ef{eq:origin})) 用另一种方式写。
这种方式避免 ({}^{C0}mathbf{R}_W) 出现在最外层矩阵乘法的两项中。于是可以方便使用 chain rule。
之所以会出现 (( ef{eq:wrong})) 从 product rule 上思考是错误的,但是得到的结果能够通过代码检查,原因是实际上使用的是 “多元复合函数求导法则”。认为 ({}^{C0}mathbf{R}_W) 对 ({}^{Ci}mathbf{R}_W) 的影响有两条路径,一条路径是通过 ({}^{Ci}mathbf{R}_W) 公式最外层矩阵乘法中的 ({}^{C0}mathbf{R}_W),一条路径是通过 (m{ au})。可以写作。(微积分常识需要巩固。)
3.1. ({partial {}^{Ci}mathbf{R}_W over partial {}^{C0}mathbf{R}_W})
注意以上以公式 (44*) 为基础,要注意 (44*) 中的 (oplus, ominus) 不完全由 (27*) 与 (28*) 定义。如 ({partial ext{Exp}(alpha m{ au}) over partial m{ au}}) 的 (oplus) 由分数线下方的 (m{ au} in mathbb{R}^3) 定义,是 vector space 的 (oplus),写作 (+)。如 ({partial m{ au} over partial {}^{C0}mathbf{R}_W}) 的 (ominus) 由分数线上方的 (m{ au} in mathbb{R}^3) 定义,是 vector space 的 (ominus),写作 (-)。
于是有(按照错误的 (( ef{eq:wrong})))。
同理,按照正确的 (( ef{eq:right0})),得到的结果如下。
3.2. ({partial {}^{Ci}mathbf{R}_W over partial {}^{C1}mathbf{R}_W})
于是有。
3.3. ({partial {}^{Ci}mathbf{R}_W over partial alpha})
于是有。
4. 代码验证
用 ceres 的 AutoDiff 与 NumericDiff 工具验证。Analytic 的结果就是公式推导的结果。
比较以下两段代码的 if(_jacobians[0] != nullptr)
。
4.1. 按照 (( ef{eq:wrong1})) 的错误示范
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/internal/autodiff.h>
#include <ceres/internal/numeric_diff.h>
#include <cstdlib>
#include <ctime>
/// use numeric diff and auto diff to check my analytic diff.
class QuaternionCostFunctor {
public:
QuaternionCostFunctor(const Eigen::Quaterniond &_ci_q_w) : ci_q_w_{_ci_q_w} {}
template <typename T>
bool operator()(const T *const _c0_q_w, const T *const _c1_q_w,
const T *const _alpha, T *_e) const {
const Eigen::Quaternion<T> c0_q_w(_c0_q_w);
const Eigen::Quaternion<T> c1_q_w(_c1_q_w);
const Eigen::Quaternion<T> delta_qua = c1_q_w * c0_q_w.inverse();
Eigen::AngleAxis<T> delta_aa(delta_qua);
delta_aa.angle() *= _alpha[0];
const Eigen::Quaternion<T> ci_q_w_p =
Eigen::Quaternion<T>(delta_aa) * c0_q_w;
const Eigen::Quaternion<T> ci_q_w(
static_cast<T>(ci_q_w_.w()), static_cast<T>(ci_q_w_.x()),
static_cast<T>(ci_q_w_.y()), static_cast<T>(ci_q_w_.z()));
const Eigen::Quaternion<T> e_q = ci_q_w_p * ci_q_w.inverse();
const Eigen::AngleAxis<T> e_aa(e_q);
Eigen::Map<Eigen::Matrix<T, 3, 1>> e(_e);
e = e_aa.axis() * e_aa.angle();
return true;
}
void evaluateAnalytically(const double *const _c0_q_w,
const double *const _c1_q_w,
const double *const _alpha, double *_e,
double **_jacobians) const {
const Eigen::Quaternion<double> c0_q_w(_c0_q_w);
const Eigen::Quaternion<double> c1_q_w(_c1_q_w);
const Eigen::Quaternion<double> delta_qua = c1_q_w * c0_q_w.inverse();
Eigen::AngleAxis<double> delta_aa(delta_qua);
delta_aa.angle() *= _alpha[0];
const Eigen::Quaternion<double> ci_q_w_p =
Eigen::Quaternion<double>(delta_aa) * c0_q_w;
const Eigen::Quaternion<double> e_q = ci_q_w_p * ci_q_w_.inverse();
const Eigen::AngleAxis<double> e_aa(e_q);
Eigen::Map<Eigen::Matrix<double, 3, 1>> e(_e);
e = e_aa.axis() * e_aa.angle();
if (_jacobians != nullptr) {
const Eigen::Vector3d tau_axis = delta_aa.axis();
const double tau_angle = delta_aa.angle() / _alpha[0];
if (_jacobians[0] != nullptr) { /// c0_q_w
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> J(
_jacobians[0]);
J.setZero();
J.block(0, 0, 3, 3) = -_alpha[0] *
J_l(_alpha[0] * tau_angle, tau_axis) *
J_r_inv(tau_angle, tau_axis) +
delta_aa.toRotationMatrix();
}
if (_jacobians[1] != nullptr) { /// c1_q_w
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> J(
_jacobians[1]);
J.setZero();
J.block(0, 0, 3, 3) = _alpha[0] * J_l(_alpha[0] * tau_angle, tau_axis) *
J_l_inv(tau_angle, tau_axis);
}
if (_jacobians[2] != nullptr) { /// alpha
Eigen::Map<Eigen::Matrix<double, 3, 1>> J(_jacobians[2]);
J = J_l(_alpha[0] * tau_angle, tau_axis) * tau_angle * tau_axis;
}
}
}
static bool GlobalToLocal(const double *x, double *jacobian) {
const double qw = x[3];
const double qx = x[0];
const double qy = x[1];
const double qz = x[2];
jacobian[0] = qw, jacobian[1] = qz, jacobian[2] = -qy;
jacobian[3] = -qz, jacobian[4] = qw, jacobian[5] = qx;
jacobian[6] = qy, jacobian[7] = -qx, jacobian[8] = qw;
jacobian[9] = -qx, jacobian[10] = -qy, jacobian[11] = -qz;
return true;
}
static inline Eigen::Matrix3d skew(const Eigen::Vector3d &_v) {
Eigen::Matrix3d res;
res.setZero();
res(0, 1) = -_v[2], res(0, 2) = _v[1], res(1, 2) = -_v[0];
res(1, 0) = _v[2], res(2, 0) = -_v[1], res(2, 1) = _v[0];
return res;
}
static Eigen::Matrix3d J_l(const double _angle,
const Eigen::Vector3d &_axis) {
/// (145)
Eigen::Matrix3d res =
Eigen::Matrix3d::Identity() +
(1 - std::cos(_angle)) / (_angle * _angle) * skew(_angle * _axis) +
(_angle - std::sin(_angle)) / (_angle * _angle * _angle) *
skew(_angle * _axis) * skew(_angle * _axis);
return res;
}
static Eigen::Matrix3d J_l_inv(const double _angle,
const Eigen::Vector3d &_axis) {
/// (146)
Eigen::Matrix3d res =
Eigen::Matrix3d::Identity() - 0.5 * skew(_angle * _axis) +
(1 / (_angle * _angle) -
(1 + std::cos(_angle)) / (2 * _angle * std::sin(_angle))) *
skew(_angle * _axis) * skew(_angle * _axis);
return res;
}
static Eigen::Matrix3d J_r(const double _angle,
const Eigen::Vector3d &_axis) {
// return J_l(_angle, _axis).transpose();
/// (143)
Eigen::Matrix3d res =
Eigen::Matrix3d::Identity() -
(1 - std::cos(_angle)) / (_angle * _angle) * skew(_angle * _axis) +
(_angle - std::sin(_angle)) / (_angle * _angle * _angle) *
skew(_angle * _axis) * skew(_angle * _axis);
return res;
}
static Eigen::Matrix3d J_r_inv(const double _angle,
const Eigen::Vector3d &_axis) {
// return J_l_inv(_angle, _axis).transpose();
/// (144)
Eigen::Matrix3d res =
Eigen::Matrix3d::Identity() + 0.5 * skew(_angle * _axis) +
(1 / (_angle * _angle) -
(1 + std::cos(_angle)) / (2 * _angle * std::sin(_angle))) *
skew(_angle * _axis) * skew(_angle * _axis);
return res;
}
private:
const Eigen::Quaterniond ci_q_w_;
};
Eigen::Quaterniond getRandomQuaternion() {
const double range = 1.;
Eigen::Vector3d axis(std::rand() / double(RAND_MAX) * 2 * range + (-range),
std::rand() / double(RAND_MAX) * 2 * range + (-range),
std::rand() / double(RAND_MAX) * 2 * range + (-range));
axis.normalize();
const double angle = std::rand() / double(RAND_MAX) * 2 * M_PI;
Eigen::AngleAxisd aa(angle, axis);
return Eigen::Quaterniond(aa);
}
int main(int argc, char **argv) {
std::srand(std::time(NULL));
std::srand(0);
Eigen::Quaterniond c0_q_w = getRandomQuaternion();
Eigen::Quaterniond c1_q_w = getRandomQuaternion();
double alpha = std::rand() / double(RAND_MAX);
std::cout << "c0_R_w:
" << c0_q_w.toRotationMatrix() << std::endl;
std::cout << "c1_R_w:
" << c1_q_w.toRotationMatrix() << std::endl;
std::cout << "alpha:
" << alpha << std::endl;
const Eigen::Quaterniond delta_qua = c1_q_w * c0_q_w.inverse();
Eigen::AngleAxisd delta_aa(delta_qua);
delta_aa.angle() *= alpha;
const Eigen::Quaterniond ci_q_w = Eigen::Quaterniond(delta_aa) * c0_q_w;
QuaternionCostFunctor functor(ci_q_w);
double residuals[3];
double *parameters[3] = {c0_q_w.coeffs().data(), c1_q_w.coeffs().data(),
&alpha};
double **jacobians = new double *[3];
for (int i = 0; i < 2; ++i)
jacobians[i] = new double[12];
jacobians[2] = new double[3];
{
ceres::internal::AutoDiff<QuaternionCostFunctor, double, 4, 4,
1>::Differentiate(functor, parameters,
3, /// residual num
residuals, jacobians);
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_0(
jacobians[0]);
Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_0;
QuaternionCostFunctor::GlobalToLocal(parameters[0],
global_to_local_0.data());
std::cout << "autodiff jacobian_0:
"
<< 0.5 * jacobian_0 * global_to_local_0 << std::endl;
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_1(
jacobians[1]);
Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_1;
QuaternionCostFunctor::GlobalToLocal(parameters[1],
global_to_local_1.data());
std::cout << "autodiff jacobian_1:
"
<< 0.5 * jacobian_1 * global_to_local_1 << std::endl;
Eigen::Map<Eigen::Matrix<double, 3, 1>> jacobian_2(jacobians[2]);
std::cout << "autodiff jacobian_2:
" << jacobian_2 << std::endl;
}
{
ceres::internal::NumericDiff<
QuaternionCostFunctor, ceres::NumericDiffMethodType::CENTRAL, 3, 4, 4,
1, 0, 0, 0, 0, 0, 0, 0, 0,
4>::EvaluateJacobianForParameterBlock(&functor, residuals,
ceres::NumericDiffOptions(),
3, /// residual num
0, /// block index
4, /// block size
parameters, jacobians[0]);
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_0(
jacobians[0]);
Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_0;
QuaternionCostFunctor::GlobalToLocal(parameters[0],
global_to_local_0.data());
std::cout << "numdiff jacobian_0:
"
<< 0.5 * jacobian_0 * global_to_local_0 << std::endl;
}
{
ceres::internal::NumericDiff<
QuaternionCostFunctor, ceres::NumericDiffMethodType::CENTRAL, 3, 4, 4,
1, 0, 0, 0, 0, 0, 0, 0, 1,
4>::EvaluateJacobianForParameterBlock(&functor, residuals,
ceres::NumericDiffOptions(),
3, /// residual num
1, /// block index
4, /// block size
parameters, jacobians[1]);
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_1(
jacobians[1]);
Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_1;
QuaternionCostFunctor::GlobalToLocal(parameters[1],
global_to_local_1.data());
std::cout << "numdiff jacobian_1:
"
<< 0.5 * jacobian_1 * global_to_local_1 << std::endl;
}
{
ceres::internal::NumericDiff<
QuaternionCostFunctor, ceres::NumericDiffMethodType::CENTRAL, 3, 4, 4,
1, 0, 0, 0, 0, 0, 0, 0, 2,
1>::EvaluateJacobianForParameterBlock(&functor, residuals,
ceres::NumericDiffOptions(),
3, /// residual num
2, /// block index
1, /// block size
parameters, jacobians[2]);
Eigen::Map<Eigen::Matrix<double, 3, 1>> jacobian_2(jacobians[2]);
std::cout << "numdiff jacobian_2:
" << jacobian_2 << std::endl;
}
{
functor.evaluateAnalytically(parameters[0], parameters[1], parameters[2],
residuals, jacobians);
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_0(
jacobians[0]);
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_1(
jacobians[1]);
Eigen::Map<Eigen::Matrix<double, 3, 1>> jacobian_2(jacobians[2]);
std::cout << "analytic jacobian_0:
"
<< jacobian_0.block(0, 0, 3, 3) << std::endl;
std::cout << "analytic jacobian_1:
"
<< jacobian_1.block(0, 0, 3, 3) << std::endl;
std::cout << "analytic jacobian_2:
" << jacobian_2 << std::endl;
}
return 0;
}
4.2. 按照 (( ef{eq:right1})) 的正确写法
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/internal/autodiff.h>
#include <ceres/internal/numeric_diff.h>
#include <cstdlib>
#include <ctime>
/// use numeric diff and auto diff to check my analytic diff.
class QuaternionCostFunctor {
public:
QuaternionCostFunctor(const Eigen::Quaterniond &_ci_q_w) : ci_q_w_{_ci_q_w} {}
template <typename T>
bool operator()(const T *const _c0_q_w, const T *const _c1_q_w,
const T *const _alpha, T *_e) const {
const Eigen::Quaternion<T> c0_q_w(_c0_q_w);
const Eigen::Quaternion<T> c1_q_w(_c1_q_w);
const Eigen::Quaternion<T> delta_qua = c1_q_w * c0_q_w.inverse();
Eigen::AngleAxis<T> delta_aa(delta_qua);
delta_aa.angle() *= _alpha[0];
const Eigen::Quaternion<T> ci_q_w_p =
Eigen::Quaternion<T>(delta_aa) * c0_q_w;
const Eigen::Quaternion<T> ci_q_w(
static_cast<T>(ci_q_w_.w()), static_cast<T>(ci_q_w_.x()),
static_cast<T>(ci_q_w_.y()), static_cast<T>(ci_q_w_.z()));
const Eigen::Quaternion<T> e_q = ci_q_w_p * ci_q_w.inverse();
const Eigen::AngleAxis<T> e_aa(e_q);
Eigen::Map<Eigen::Matrix<T, 3, 1>> e(_e);
e = e_aa.axis() * e_aa.angle();
return true;
}
void evaluateAnalytically(const double *const _c0_q_w,
const double *const _c1_q_w,
const double *const _alpha, double *_e,
double **_jacobians) const {
const Eigen::Quaternion<double> c0_q_w(_c0_q_w);
const Eigen::Quaternion<double> c1_q_w(_c1_q_w);
const Eigen::Quaternion<double> delta_qua = c1_q_w * c0_q_w.inverse();
Eigen::AngleAxis<double> delta_aa(delta_qua);
delta_aa.angle() *= _alpha[0];
const Eigen::Quaternion<double> ci_q_w_p =
Eigen::Quaternion<double>(delta_aa) * c0_q_w;
const Eigen::Quaternion<double> e_q = ci_q_w_p * ci_q_w_.inverse();
const Eigen::AngleAxis<double> e_aa(e_q);
Eigen::Map<Eigen::Matrix<double, 3, 1>> e(_e);
e = e_aa.axis() * e_aa.angle();
if (_jacobians != nullptr) {
const Eigen::Vector3d tau_axis = delta_aa.axis();
const double tau_angle = delta_aa.angle() / _alpha[0];
if (_jacobians[0] != nullptr) { /// c0_q_w
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> J(
_jacobians[0]);
J.setZero();
J.block(0, 0, 3, 3) = -(_alpha[0] - 1) *
J_l((_alpha[0] - 1) * tau_angle, tau_axis) *
J_r_inv(tau_angle, tau_axis);
}
if (_jacobians[1] != nullptr) { /// c1_q_w
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> J(
_jacobians[1]);
J.setZero();
J.block(0, 0, 3, 3) = _alpha[0] * J_l(_alpha[0] * tau_angle, tau_axis) *
J_l_inv(tau_angle, tau_axis);
}
if (_jacobians[2] != nullptr) { /// alpha
Eigen::Map<Eigen::Matrix<double, 3, 1>> J(_jacobians[2]);
J = J_l(_alpha[0] * tau_angle, tau_axis) * tau_angle * tau_axis;
}
}
}
static bool GlobalToLocal(const double *x, double *jacobian) {
const double qw = x[3];
const double qx = x[0];
const double qy = x[1];
const double qz = x[2];
jacobian[0] = qw, jacobian[1] = qz, jacobian[2] = -qy;
jacobian[3] = -qz, jacobian[4] = qw, jacobian[5] = qx;
jacobian[6] = qy, jacobian[7] = -qx, jacobian[8] = qw;
jacobian[9] = -qx, jacobian[10] = -qy, jacobian[11] = -qz;
return true;
}
static inline Eigen::Matrix3d skew(const Eigen::Vector3d &_v) {
Eigen::Matrix3d res;
res.setZero();
res(0, 1) = -_v[2], res(0, 2) = _v[1], res(1, 2) = -_v[0];
res(1, 0) = _v[2], res(2, 0) = -_v[1], res(2, 1) = _v[0];
return res;
}
static Eigen::Matrix3d J_l(const double _angle,
const Eigen::Vector3d &_axis) {
/// (145)
Eigen::Matrix3d res =
Eigen::Matrix3d::Identity() +
(1 - std::cos(_angle)) / (_angle * _angle) * skew(_angle * _axis) +
(_angle - std::sin(_angle)) / (_angle * _angle * _angle) *
skew(_angle * _axis) * skew(_angle * _axis);
return res;
}
static Eigen::Matrix3d J_l_inv(const double _angle,
const Eigen::Vector3d &_axis) {
/// (146)
Eigen::Matrix3d res =
Eigen::Matrix3d::Identity() - 0.5 * skew(_angle * _axis) +
(1 / (_angle * _angle) -
(1 + std::cos(_angle)) / (2 * _angle * std::sin(_angle))) *
skew(_angle * _axis) * skew(_angle * _axis);
return res;
}
static Eigen::Matrix3d J_r(const double _angle,
const Eigen::Vector3d &_axis) {
return J_l(_angle, _axis).transpose();
/// (143)
Eigen::Matrix3d res =
Eigen::Matrix3d::Identity() -
(1 - std::cos(_angle)) / (_angle * _angle) * skew(_angle * _axis) +
(_angle - std::sin(_angle)) / (_angle * _angle * _angle) *
skew(_angle * _axis) * skew(_angle * _axis);
return res;
}
static Eigen::Matrix3d J_r_inv(const double _angle,
const Eigen::Vector3d &_axis) {
return J_l_inv(_angle, _axis).transpose();
/// (144)
Eigen::Matrix3d res =
Eigen::Matrix3d::Identity() + 0.5 * skew(_angle * _axis) +
(1 / (_angle * _angle) -
(1 + std::cos(_angle)) / (2 * _angle * std::sin(_angle))) *
skew(_angle * _axis) * skew(_angle * _axis);
return res;
}
private:
const Eigen::Quaterniond ci_q_w_;
};
Eigen::Quaterniond getRandomQuaternion() {
const double range = 1.;
Eigen::Vector3d axis(std::rand() / double(RAND_MAX) * 2 * range + (-range),
std::rand() / double(RAND_MAX) * 2 * range + (-range),
std::rand() / double(RAND_MAX) * 2 * range + (-range));
axis.normalize();
const double angle = std::rand() / double(RAND_MAX) * 2 * M_PI;
Eigen::AngleAxisd aa(angle, axis);
return Eigen::Quaterniond(aa);
}
int main(int argc, char **argv) {
std::srand(std::time(NULL));
std::srand(0);
Eigen::Quaterniond c0_q_w = getRandomQuaternion();
Eigen::Quaterniond c1_q_w = getRandomQuaternion();
double alpha = std::rand() / double(RAND_MAX);
std::cout << "c0_R_w:
" << c0_q_w.toRotationMatrix() << std::endl;
std::cout << "c1_R_w:
" << c1_q_w.toRotationMatrix() << std::endl;
std::cout << "alpha:
" << alpha << std::endl;
const Eigen::Quaterniond delta_qua = c1_q_w * c0_q_w.inverse();
Eigen::AngleAxisd delta_aa(delta_qua);
delta_aa.angle() *= alpha;
const Eigen::Quaterniond ci_q_w = Eigen::Quaterniond(delta_aa) * c0_q_w;
std::cout << "ci_R_w:
" << ci_q_w.toRotationMatrix() << std::endl;
QuaternionCostFunctor functor(ci_q_w);
double residuals[3];
double *parameters[3] = {c0_q_w.coeffs().data(), c1_q_w.coeffs().data(),
&alpha};
double **jacobians = new double *[3];
for (int i = 0; i < 2; ++i)
jacobians[i] = new double[12];
jacobians[2] = new double[3];
{
ceres::internal::AutoDiff<QuaternionCostFunctor, double, 4, 4,
1>::Differentiate(functor, parameters,
3, /// residual num
residuals, jacobians);
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_0(
jacobians[0]);
Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_0;
QuaternionCostFunctor::GlobalToLocal(parameters[0],
global_to_local_0.data());
std::cout << "autodiff jacobian_0:
"
<< 0.5 * jacobian_0 * global_to_local_0 << std::endl;
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_1(
jacobians[1]);
Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_1;
QuaternionCostFunctor::GlobalToLocal(parameters[1],
global_to_local_1.data());
std::cout << "autodiff jacobian_1:
"
<< 0.5 * jacobian_1 * global_to_local_1 << std::endl;
Eigen::Map<Eigen::Matrix<double, 3, 1>> jacobian_2(jacobians[2]);
std::cout << "autodiff jacobian_2:
" << jacobian_2 << std::endl;
}
{
ceres::internal::NumericDiff<
QuaternionCostFunctor, ceres::NumericDiffMethodType::CENTRAL, 3, 4, 4,
1, 0, 0, 0, 0, 0, 0, 0, 0,
4>::EvaluateJacobianForParameterBlock(&functor, residuals,
ceres::NumericDiffOptions(),
3, /// residual num
0, /// block index
4, /// block size
parameters, jacobians[0]);
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_0(
jacobians[0]);
Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_0;
QuaternionCostFunctor::GlobalToLocal(parameters[0],
global_to_local_0.data());
std::cout << "numdiff jacobian_0:
"
<< 0.5 * jacobian_0 * global_to_local_0 << std::endl;
}
{
ceres::internal::NumericDiff<
QuaternionCostFunctor, ceres::NumericDiffMethodType::CENTRAL, 3, 4, 4,
1, 0, 0, 0, 0, 0, 0, 0, 1,
4>::EvaluateJacobianForParameterBlock(&functor, residuals,
ceres::NumericDiffOptions(),
3, /// residual num
1, /// block index
4, /// block size
parameters, jacobians[1]);
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_1(
jacobians[1]);
Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_1;
QuaternionCostFunctor::GlobalToLocal(parameters[1],
global_to_local_1.data());
std::cout << "numdiff jacobian_1:
"
<< 0.5 * jacobian_1 * global_to_local_1 << std::endl;
}
{
ceres::internal::NumericDiff<
QuaternionCostFunctor, ceres::NumericDiffMethodType::CENTRAL, 3, 4, 4,
1, 0, 0, 0, 0, 0, 0, 0, 2,
1>::EvaluateJacobianForParameterBlock(&functor, residuals,
ceres::NumericDiffOptions(),
3, /// residual num
2, /// block index
1, /// block size
parameters, jacobians[2]);
Eigen::Map<Eigen::Matrix<double, 3, 1>> jacobian_2(jacobians[2]);
std::cout << "numdiff jacobian_2:
" << jacobian_2 << std::endl;
}
{
functor.evaluateAnalytically(parameters[0], parameters[1], parameters[2],
residuals, jacobians);
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_0(
jacobians[0]);
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_1(
jacobians[1]);
Eigen::Map<Eigen::Matrix<double, 3, 1>> jacobian_2(jacobians[2]);
std::cout << "analytic jacobian_0:
"
<< jacobian_0.block(0, 0, 3, 3) << std::endl;
std::cout << "analytic jacobian_1:
"
<< jacobian_1.block(0, 0, 3, 3) << std::endl;
std::cout << "analytic jacobian_2:
" << jacobian_2 << std::endl;
}
return 0;
}