30 using namespace gtsam;
39 auto model = noiseModel::Isotropic::Sigma(9, 1);
40 auto model2 = noiseModel::Isotropic::Sigma(3, 1);
51 if (
H) *
H = m_ * Matrix::Identity(
X.rows() *
X.cols(),
X.rows() *
X.cols());
63 H1->resize(
x.size(),
A.size());
64 *H1 << I_3x3, I_3x3, I_3x3;
76 double multiplier = 1.0;
88 double multiplier = 2.0;
89 Matrix X = Matrix::Identity(3, 3);
105 double multiplier = 2.0;
118 Matrix X = Matrix::Identity(3, 3);
121 double multiplier = 2.0;
136 Matrix X = Matrix::Identity(2, 2);
138 double multiplier = 2.0;
145 " noise model: unit (9) \n"
146 "FunctorizedFactor(X0)\n"
151 " noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
159 double multiplier = 2.0;
160 Matrix X = Matrix::Identity(3, 3);
176 double multiplier = 2.0;
177 Matrix X = Matrix::Identity(3, 3);
184 Matrix::Identity(
X.rows() *
X.cols(),
X.rows() *
X.cols());
185 return multiplier *
X;
199 Matrix A = Matrix::Identity(3, 3);
204 MakeFunctorizedFactor2<Matrix, Vector>(
keyA,
keyx,
x,
model2, functor);
214 Matrix A = Matrix::Identity(3, 3);
216 Matrix actualH1, actualH2;
218 auto factor = MakeFunctorizedFactor2<Matrix, Vector>(
keyA,
keyx,
x,
model2,
232 Matrix A = Matrix::Identity(3, 3);
249 Matrix A = Matrix::Identity(3, 3);
257 H1->resize(
x.size(),
A.size());
258 *H1 << I_3x3, I_3x3, I_3x3;