28 using namespace gtsam;
37 const vector<pair<Key, Matrix> > terms{
38 {0, 1 * I_3x3}, {1, 2 * I_3x3}, {2, 3 * I_3x3}};
42 const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((
Vector(3) << 0.5,0.5,0.5).finished());
47 const vector<pair<Key, Matrix> > terms2{
48 {0, 2 * I_3x3}, {1, 4 * I_3x3}, {2, 6 * I_3x3}};
58 const size_t nrKeys,
const size_t dim) {
60 std::map<gtsam::Key, size_t> dims;
66 for (
size_t i = 0;
i <
n;
i++){
74 const size_t nrKeys,
const size_t dim) {
76 std::map<gtsam::Key, size_t> dims;
78 dims.insert(make_pair(
i, dim));
82 for (
size_t j = 0;
j <
n;
j++)
91 terms[1].first, terms[1].second, terms[2].first, terms[2].second,
b, noise);
107 terms[1].first, terms[1].second, terms[2].first, terms[2].second,
b, noise);
114 double actualValue[9]={0};
121 TEST(RegularJacobian, gradientAtZero)
125 terms[1].first, terms[1].second, terms[2].first, terms[2].second,
b, noise);
134 double actualValue[9]={0};
141 TEST(RegularJacobian, gradientAtZero_multiFactors)
145 terms[1].first, terms[1].second, terms[2].first, terms[2].second,
b, noise);
152 double actualValue[9]={0};
160 terms2[1].first, terms2[1].second, terms2[2].first, terms2[2].second,
b2, noise);
174 TEST(RegularJacobian, multiplyHessianAdd)
178 terms[1].first, terms[1].second, terms[2].first, terms[2].second,
b, noise);
183 X.insert(0, (
Vector(3) << 10.,20.,30.).finished());
184 X.insert(1, (
Vector(3) << 10.,20.,30.).finished());
185 X.insert(2, (
Vector(3) << 10.,20.,30.).finished());
189 Y.insert(0, (
Vector(3) << 10.,10.,10.).finished());
190 Y.insert(1, (
Vector(3) << 20.,20.,20.).finished());
191 Y.insert(2, (
Vector(3) << 30.,30.,30.).finished());
203 double actualMHARaw[9];
210 double actualMHARaw2[9];
213 size_t accumulatedDim = 0;
215 dims.push_back(accumulatedDim);