Kernel.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/dllexport.h>
 
#include <limits>
 
namespace gtsam {
namespace so3 {
 
struct DexpFunctor;

struct GTSAM_EXPORT Kernel {
  const DexpFunctor* S;
  double a{0}, b{0}, c{0}, db{0}, dc{0};  // left-specialization form
 
  Matrix3 left() const;   // a I + b Ω + c Ω²
  Matrix3 right() const;  // a I - b Ω + c Ω²
 
  Vector3 applyLeft(const Vector3& v, OptionalJacobian<3, 3> Hw = {},
                    OptionalJacobian<3, 3> Hv = {}) const;
  Vector3 applyRight(const Vector3& v, OptionalJacobian<3, 3> Hw = {},
                     OptionalJacobian<3, 3> Hv = {}) const;

  Matrix3 frechet(const Matrix3& X) const;
  Matrix3 applyFrechet(const Vector3& v) const;
};
 
// Stable inverse Jacobian kernel
struct GTSAM_EXPORT InvJKernel {
  const DexpFunctor* S;
  Kernel J;  // holds the forward kernel
 
  Matrix3 left() const;
  Matrix3 right() const;
 
  Vector3 applyLeft(const Vector3& v, OptionalJacobian<3, 3> Hw = {},
                    OptionalJacobian<3, 3> Hv = {}) const;
  Vector3 applyRight(const Vector3& v, OptionalJacobian<3, 3> Hw = {},
                     OptionalJacobian<3, 3> Hv = {}) const;
};

GTSAM_EXPORT Kernel axpy(double alpha, const Kernel& X, const Kernel& Y);
 
// Blend K = α X + (1-α) Y with radial derivative (·)'/θ via dalpha
GTSAM_EXPORT Kernel blend(double alpha, double dalpha, const Kernel& X,
                          const Kernel& Y);
 
}  // namespace so3
}  // namespace gtsam