59 std::optional<double> gamma = {})
62 throw std::invalid_argument(
"ShonanGaugeFactor must have p>=d.");
77 double invSigma = gamma ?
std::sqrt(*gamma) : 1.0;
78 size_t i = 0,
j = 0,
n = p - 1 -
d;
80 A.block(i,
j,
n,
n) = invSigma * Matrix::Identity(
n,
n);
87 std::make_shared<JacobianFactor>(
key, A, Vector::Zero(rows_));
97 size_t dim()
const override {
return rows_; }
101 return whitenedJacobian_;
const gtsam::Symbol key('X', 0)
double error(const Values &c) const override
Calculate the error of the factor: always zero.
ShonanGaugeFactor(Key key, size_t p, size_t d=3, std::optional< double > gamma={})
static size_t Dimension(size_t n)
std::shared_ptr< GaussianFactor > linearize(const Values &c) const override
linearize to a GaussianFactor
EIGEN_DEVICE_FUNC const Scalar & q
~ShonanGaugeFactor() override
Destructor.
std::shared_ptr< JacobianFactor > whitenedJacobian_
Constant Jacobian.
Non-linear factor base classes.
Jet< T, N > sqrt(const Jet< T, N > &f)
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
N*N matrix representation of SO(N). N can be Eigen::Dynamic.
std::uint64_t Key
Integer nonlinear key type.
size_t dim() const override
get the dimension of the factor (number of rows on linearization)