Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

why does autodiff produce different jacobian w.r.t analytic, but give the same result #1037

Open
EXing opened this issue Dec 19, 2023 · 4 comments

Comments

@EXing
Copy link

EXing commented Dec 19, 2023

struct QuatMulVec {
  QuatMulVec(const Eigen::Vector3d& v, bool use_eigen)
      : v_(v), use_eigen_(use_eigen) {}

  template <typename T>
  static Eigen::Vector3<T> Eval(
      const Eigen::Quaternion<T>& Q,
      const typename Eigen::Vector3<T>::PlainObject& v) {
    const auto& x = Q.x();
    const auto& y = Q.y();
    const auto& z = Q.z();
    const auto& w = Q.w();
    const auto& a = v[0];
    const auto& b = v[1];
    const auto& c = v[2];

    Eigen::Vector3<T> p;
    p[0] = a * (w * w + x * x - y * y - z * z) + T(2) * b * (x * y - w * z) +
           T(2) * c * (w * y + x * z);
    p[1] = T(2) * a * (w * z + x * y) + b * (w * w - x * x + y * y - z * z) +
           T(2) * c * (y * z - w * x);
    p[2] = T(2) * a * (x * z - w * y) + T(2) * b * (w * x + y * z) +
           c * (w * w - x * x - y * y + z * z);
    return p;
  }

  template <typename T>
  static Eigen::Matrix<T, 3, 4> JacobianByQ(const Eigen::Quaternion<T>& Q,
                                            const Eigen::Vector3<T>& v) {
    const auto& x = Q.x();
    const auto& y = Q.y();
    const auto& z = Q.z();
    const auto& w = Q.w();
    const auto& a = v[0];
    const auto& b = v[1];
    const auto& c = v[2];
    auto bw_minus_cx_add_az = b * w - c * x + a * z;
    auto aw_add_cy_minus_bz = a * w + c * y - b * z;
    auto ax_add_by_add_cz = a * x + b * y + c * z;
    auto cw_add_bx_minus_ay = c * w + b * x - a * y;

    Eigen::Matrix<T, 3, 4> J;
    // a*(w^2+x^2-y^2-z^2)+2*b*(x*y-w*z)+2*c*(w*y+x*z)
    J(0, 0) = 2 * (ax_add_by_add_cz);
    J(0, 1) = 2 * (cw_add_bx_minus_ay);
    J(0, 2) = -2 * (bw_minus_cx_add_az);
    J(0, 3) = 2 * (aw_add_cy_minus_bz);

    // 2*a*(w*z+x*y)+b*(w^2-x^2+y^2-z^2)+2*c*(y*z-w*x)
    J(1, 0) = -2 * (cw_add_bx_minus_ay);
    J(1, 1) = 2 * (ax_add_by_add_cz);
    J(1, 2) = 2 * (aw_add_cy_minus_bz);
    J(1, 3) = 2 * (bw_minus_cx_add_az);

    // 2*a*(x*z-w*y)+2*b*(w*x+y*z)+c*(w^2-x^2-y^2+z^2)
    J(2, 0) = 2 * (bw_minus_cx_add_az);
    J(2, 1) = -2 * (aw_add_cy_minus_bz);
    J(2, 2) = 2 * (ax_add_by_add_cz);
    J(2, 3) = 2 * (cw_add_bx_minus_ay);
    return J;
  }

  template <typename T>
  bool operator()(const T* const q, T* output) const {
    const Eigen::Quaternion<T> Q(q);
    Eigen::Map<Eigen::Vector3<T>> v(output);
    if (use_eigen_)
      v = Q * v_.cast<T>();
    else
      v = Eval(Q, v_.cast<T>());
    return true;
  }

  static ceres::CostFunction* Create(const Eigen::Vector3d& v, bool use_eigen) {
    return (new ceres::AutoDiffCostFunction<QuatMulVec, 3, 4>(
        new QuatMulVec(v, use_eigen)));
  }

  Eigen::Vector3d v_;
  bool use_eigen_;
};
TEST(QuatMulVec, JacobianByQ) {
  Eigen::Quaterniond Q = Eigen::Quaterniond::UnitRandom();
  Eigen::Vector3d v = Eigen::Vector3d::Random();

  Eigen::Matrix<double, 3, 4> J_analytic = QuatMulVec::JacobianByQ(Q, v);
  Eigen::Matrix<double, 12, 1> jacobian = Eigen::Matrix<double, 12, 1>::Zero();
  double* jacobians[1] = {jacobian.data()};
  const double* parameters[1] = {Q.coeffs().data()};

  ceres::CostFunction* cf_analytic = QuatMulVec::Create(v, false);
  Eigen::Vector3d v_analytic;
  cf_analytic->Evaluate(parameters, v_analytic.data(), jacobians);
  Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajorBit>>
      J_analytic_autodiff(jacobian.data());
  EXPECT_TRUE(J_analytic.isApprox(J_analytic_autodiff));

  ceres::CostFunction* cf_eigen = QuatMulVec::Create(v, true);
  Eigen::Vector3d v_eigen;
  cf_eigen->Evaluate(parameters, v_eigen.data(), jacobians);
  Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajorBit>> J_autodiff(
      jacobian.data());
  EXPECT_TRUE(v_analytic.isApprox(v_eigen));
  EXPECT_TRUE(J_analytic.isApprox(J_autodiff));
}

J_analytic == J_analytic_autodiff != J_autodiff and v_analytic == v_eigen

@EXing
Copy link
Author

EXing commented Dec 19, 2023

Ceres version: 2.2.0
Eigen version: 3.4.0

@EXing EXing closed this as completed Dec 19, 2023
@EXing EXing changed the title Bug in autodiff jacobian evaluation why does autodiff produce different jacobian w.r.t analytic, but give the same result Dec 19, 2023
@EXing EXing reopened this Dec 19, 2023
@sergiud
Copy link
Contributor

sergiud commented Dec 19, 2023

The reason for different derivatives is that operator* of an Eigen::Quaternion when applied to a vector is not implemented in terms of a Hamilton product by evaluating the conjugation $\mathbf p' = \mathbf q \mathbf p \mathbf q^*$ but as a rotation matrix/vector multiplication.

@EXing
Copy link
Author

EXing commented Dec 19, 2023

Thanks for your reply.
It use QuaternionBase::_transformVector.
Could you please explain why different methods produce very different derivatives or give some reference?
@sergiud

Hamilton product

 -2.08168  0.254191 -0.250392   1.06735
-0.254191  -2.08168   1.06735  0.250392
 0.250392  -1.06735  -2.08168  0.254191

_transformVector:

 -1.70573   1.32483 -0.476319  0.773397
  0.26436 -0.604905  0.755723 -0.155067
-0.130601  -2.15237  -1.85272  0.552093

The reason for different derivatives is that operator* of an Eigen::Quaternion when applied to a vector is not implemented in terms of a Hamilton product by evaluating the conjugation p′=qpq∗ but as a rotation matrix/vector multiplication.

@sergiud
Copy link
Contributor

sergiud commented Dec 19, 2023

True, there's QuaternionBase::_transformVector which I previously missed. Still, my explanation holds as the comment in the implementation states the following:

Note that this algorithm comes from the optimization by hand of the conversion to a Matrix followed by a Matrix/Vector product. It appears to be much faster than the common algorithm found in the literature (30 versus 39 flops). It also requires two Vector3 as temporaries.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants