27 #include <boost/assign/std/vector.hpp> 28 #include <boost/assign/list_of.hpp> 29 #include <boost/range/iterator_range.hpp> 30 #include <boost/range/adaptor/map.hpp> 33 using namespace gtsam;
43 const vector<pair<Key, Matrix> > terms = list_of<pair<Key,Matrix> >
44 (make_pair(0, Matrix3::Identity()))
45 (make_pair(1, 2*Matrix3::Identity()))
46 (make_pair(2, 3*Matrix3::Identity()));
50 const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((
Vector(3) << 0.5,0.5,0.5).finished());
55 const vector<pair<Key, Matrix> > terms2 = list_of<pair<Key,Matrix> >
56 (make_pair(0, 2*Matrix3::Identity()))
57 (make_pair(1, 4*Matrix3::Identity()))
58 (make_pair(2, 6*Matrix3::Identity()));
70 std::map<gtsam::Key, size_t> dims;
74 size_t n = nrKeys*
dim;
76 for (
size_t i = 0;
i <
n;
i++){
86 std::map<gtsam::Key, size_t> dims;
88 dims.insert(make_pair(
i, dim));
91 size_t n = nrKeys*
dim;
92 for (
size_t j = 0;
j <
n;
j++)
101 terms[1].first, terms[1].second, terms[2].first, terms[2].second,
b, noise);
117 terms[1].first, terms[1].second, terms[2].first, terms[2].second,
b, noise);
124 double actualValue[9]={0};
131 TEST(RegularJacobian, gradientAtZero)
135 terms[1].first, terms[1].second, terms[2].first, terms[2].second,
b, noise);
144 double actualValue[9]={0};
151 TEST(RegularJacobian, gradientAtZero_multiFactors)
155 terms[1].first, terms[1].second, terms[2].first, terms[2].second,
b, noise);
162 double actualValue[9]={0};
170 terms2[1].first, terms2[1].second, terms2[2].first, terms2[2].second,
b2, noise);
184 TEST(RegularJacobian, multiplyHessianAdd)
188 terms[1].first, terms[1].second, terms[2].first, terms[2].second,
b, noise);
213 double actualMHARaw[9];
220 double actualMHARaw2[9];
223 size_t accumulatedDim = 0;
225 dims.push_back(accumulatedDim);
Provides additional testing facilities for common data structures.
static const size_t fixedDim
static int runAllTests(TestResult &result)
const_iterator end() const
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
TEST(RegularJacobianFactor, constructorNway)
static const size_t nrKeys
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
VectorValues gradientAtZero() const override
Expose base class gradientAtZero.
constexpr int first(int i)
Implementation details for constexpr functions.
const constBVector getb() const
#define EXPECT(condition)
VectorValues add(const VectorValues &c) const
JacobianFactor class with fixed sized blcoks.
Conditional Gaussian Base class.
Linear Factor Graph where all factors are Gaussians.
constABlock getA(const_iterator variable) const
const SharedDiagonal & get_model() const
#define LONGS_EQUAL(expected, actual)
const mpreal dim(const mpreal &a, const mpreal &b, mp_rnd_t r=mpreal::get_default_rnd())
noiseModel::Diagonal::shared_ptr SharedDiagonal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
void vv2double(const VectorValues &vv, double *y, const size_t nrKeys, const size_t dim)
const KeyVector & keys() const
Access the factor's involved variable keys.
void hessianDiagonal(double *d) const override
Raw memory access version of hessianDiagonal.
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))
VectorValues gradientAtZero() const override
A'*b for Jacobian.
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
iterator insert(const std::pair< Key, Vector > &key_value)
VectorValues double2vv(const double *x, const size_t nrKeys, const size_t dim)
void hessianDiagonal(double *d) const override
Raw memory access version of hessianDiagonal.