Function QuaternionError

src/Geometry.cpp:37–130

  }

  Vector3 QuaternionError(const Quaternion Q1,
                          const Quaternion Q2,
                          Matrix34 *Jacobian1,
                          Matrix34 *Jacobian2)
  {
    if(Jacobian1 != nullptr && Jacobian2 != nullptr)
    {
      /** we want both jacobians */
      typedef ceres::Jet<double, 8> JetType;

      /** init jets */
      QuaternionT<JetType> Q1Jet = Q1.template cast<JetType>();
      Q1Jet.x().v(0) = 1;
      Q1Jet.y().v(1) = 1;
      Q1Jet.z().v(2) = 1;
      Q1Jet.w().v(3) = 1;
      QuaternionT<JetType> Q2Jet = Q2.template cast<JetType>();
      Q1Jet.x().v(4) = 1;
      Q1Jet.y().v(5) = 1;
      Q1Jet.z().v(6) = 1;
      Q1Jet.w().v(7) = 1;

      /** apply transformation */
      const VectorT<JetType,3> ErrorJet = QuaternionError<JetType>(Q1Jet, Q2Jet);

      /** extract jacobians */
      Jacobian1->row(0) = ErrorJet(0).v.head(4);
      Jacobian1->row(1) = ErrorJet(1).v.head(4);
      Jacobian1->row(2) = ErrorJet(2).v.head(4);

      Jacobian2->row(0) = ErrorJet(0).v.tail(4);
      Jacobian2->row(1) = ErrorJet(1).v.tail(4);
      Jacobian2->row(2) = ErrorJet(2).v.tail(4);

      /** extract mean */
      Vector3 Error;
      Error << ErrorJet(0).a, ErrorJet(1).a, ErrorJet(2).a;
      return Error;
    }
    else if(Jacobian1 != nullptr && Jacobian2 == nullptr)
    {
      /** we want just jacobian 1 */
      typedef ceres::Jet<double, 4> JetType;

      /** init jets */
      QuaternionT<JetType> Q1Jet = Q1.template cast<JetType>();
      Q1Jet.x().v(0) = 1;
      Q1Jet.y().v(1) = 1;
      Q1Jet.z().v(2) = 1;
      Q1Jet.w().v(3) = 1;

      /** apply transformation */
      const VectorT<JetType,3> ErrorJet = QuaternionError<JetType>(Q1Jet, Q2.template cast<JetType>());

      /** extract jacobians */
      Jacobian1->row(0) = ErrorJet(0).v;
      Jacobian1->row(1) = ErrorJet(1).v;
      Jacobian1->row(2) = ErrorJet(2).v;