Go to the documentation of this file.
37 const std::shared_ptr<Matrix> &
G)
44 throw std::invalid_argument(
45 "ShonanFactor: model with incorrect dimension.");
47 G_ = std::make_shared<Matrix>();
50 if (
static_cast<size_t>(
G_->rows()) !=
pp_ ||
52 throw std::invalid_argument(
"ShonanFactor: passed in generators "
53 "of incorrect dimension.");
60 std::cout <<
s <<
"ShonanFactor<" << p_ <<
">(" << keyFormatter(key<1>()) <<
","
61 << keyFormatter(key<2>()) <<
")\n";
63 noiseModel_->print(
" noise model: ");
72 p_ ==
e->p_ && M_ ==
e->M_;
80 gttic(ShonanFactor_Jacobians);
81 const size_t dim = p_ *
d;
87 *H1 = Matrix::Zero(dim, G_->cols());
88 for (
size_t j = 0;
j <
d;
j++) {
89 auto MG_j =
M1 * G_->middleRows(
j * p_, p_);
90 for (
size_t i = 0;
i <
d;
i++) {
91 H1->middleRows(
i * p_, p_) -= M_(
j,
i) * MG_j;
99 H2->resize(dim, G_->cols());
100 for (
size_t i = 0;
i <
d;
i++) {
101 H2->middleRows(
i * p_, p_) =
M2 * G_->middleRows(
i * p_, p_);
111 gttic(ShonanFactor_evaluateError);
115 if (
M1.rows() !=
static_cast<int>(p_) ||
M2.rows() !=
static_cast<int>(p_))
116 throw std::invalid_argument(
"Invalid dimension SOn values passed to "
117 "ShonanFactor<d>::evaluateError");
119 const size_t dim = p_ *
d;
120 Vector fQ2(dim), hQ1(dim);
123 fQ2 << Eigen::Map<const Matrix>(
M2.data(), dim, 1);
126 const Matrix Q1PR12 =
M1.leftCols<
d>() * M_;
127 hQ1 << Eigen::Map<const Matrix>(Q1PR12.data(), dim, 1);
129 this->fillJacobians(
M1,
M2, H1, H2);
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
size_t dim() const override
std::shared_ptr< Matrix > G_
matrix of vectorized generators
M1<< 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;Map< MatrixXf > M2(M1.data(), 6, 2)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double d[K][N]
static size_t Dimension(size_t n)
Main factor type in Shonan averaging, on SO(n) pairs.
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
size_t pp_
dimensionality constants
void fillJacobians(const Matrix &M1, const Matrix &M2, OptionalMatrixType H1, OptionalMatrixType H2) const
Calculate Jacobians if asked, Only implemented for d=2 and 3 in .cpp.
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
assert equality up to a tolerance
const SharedNoiseModel & noiseModel() const
access to the noise model
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Vector evaluateError(const SOn &Q1, const SOn &Q2, OptionalMatrixType H1, OptionalMatrixType H2) const override
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
typename std::conditional< d==2, Rot2, Rot3 >::type Rot
noiseModel::Base::shared_ptr SharedNoiseModel
noiseModel::Diagonal::shared_ptr model
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit)
Factor Graph consisting of non-linear factors.
JacobiRotation< float > G
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Matrix * OptionalMatrixType
std::uint64_t Key
Integer nonlinear key type.
Various factors that minimize some Frobenius norm.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:15